Initial commit

engines/hpl1/engine/libraries/newton/NewtonClass.cpp

@@ -0,0 +1,271 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+ *
+ * This software is provided 'as-is', without any express or implied
+ * warranty. In no event will the authors be held liable for any damages
+ * arising from the use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not
+ * claim that you wrote the original software. If you use this software
+ * in a product, an acknowledgment in the product documentation would be
+ * appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ * misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "NewtonStdAfx.h"
+#include "NewtonClass.h"
+
+NewtonDeadBodies::NewtonDeadBodies(dgMemoryAllocator *const allocator) :
+	dgTree<dgBody *, void *>(allocator) {
+	Insert((dgBody *) NULL, 0);
+}
+
+void NewtonDeadBodies::DestroyBodies(Newton &world) {
+	dgBody *body;
+	dgTreeNode *node;
+
+	Iterator iter(*this);
+	for (iter.Begin(); iter;) {
+		node = iter.GetNode();
+		iter++;
+		body = node->GetInfo();
+		if (body) {
+			Remove(node);
+			world.DestroyBody(body);
+		}
+	}
+}
+
+NewtonDeadJoints::NewtonDeadJoints(dgMemoryAllocator *const allocator) :
+	dgTree<dgConstraint *, void *>(allocator) {
+	Insert((dgConstraint *) NULL, 0);
+}
+
+void NewtonDeadJoints::DestroyJoints(Newton &world) {
+	dgTreeNode *node;
+	dgConstraint *joint;
+
+	Iterator iter(*this);
+	for (iter.Begin(); iter;) {
+		node = iter.GetNode();
+		iter++;
+		joint = node->GetInfo();
+		if (joint) {
+			Remove(node);
+			world.DestroyConstraint(joint);
+		}
+	}
+}
+
+void *Newton::DefaultAllocMemory(dgInt32 size) {
+	return malloc(size_t(size));
+}
+
+void Newton::DefaultFreeMemory(void *ptr, dgInt32 size) {
+	free(ptr);
+}
+
+Newton::Newton(dgFloat32 scale, dgMemoryAllocator *const allocator) :
+	dgWorld(allocator), NewtonDeadBodies(allocator), NewtonDeadJoints(allocator) {
+	m_updating = false;
+	g_maxTimeStep = dgFloat32(1.0f / 60.0f);
+
+	m_destructor = NULL;
+//	SetGlobalScale (scale);
+}
+
+Newton::~Newton() {
+	if (m_destructor) {
+		m_destructor((NewtonWorld *) this);
+	}
+}
+
+void Newton::UpdatePhysics(dgFloat32 timestep) {
+	m_updating = true;
+	Update(timestep);
+
+//	RagdollHeaderActiveList::UpdateMatrix();
+	m_updating = false;
+
+	NewtonDeadBodies &bodyList = *this;
+	NewtonDeadJoints &jointList = *this;
+
+	jointList.DestroyJoints(*this);
+	bodyList.DestroyBodies(*this);
+}
+
+void Newton::DestroyJoint(dgConstraint *joint) {
+	if (m_updating) {
+		NewtonDeadJoints &jointList = *this;
+		jointList.Insert(joint, joint);
+	} else {
+		dgWorld::DestroyConstraint(joint);
+	}
+}
+
+void Newton::DestroyBody(dgBody *body) {
+	if (m_updating) {
+		NewtonDeadBodies &bodyList = *this;
+		bodyList.Insert(body, body);
+	} else {
+		dgWorld::DestroyBody(body);
+	}
+}
+
+NewtonUserJoint::NewtonUserJoint(dgWorld *world, dgInt32 maxDof,
+                                 NewtonUserBilateralCallBack callback,
+                                 NewtonUserBilateralGetInfoCallBack getInfo, dgBody *dyn0, dgBody *dyn1) :
+	dgUserConstraint(world, dyn0, dyn1, 1) {
+	m_rows = 0;
+	m_maxDOF = dgUnsigned8(maxDof);
+	m_jacobianFnt = callback;
+	m_getInfoCallback = getInfo;
+
+	NEWTON_ASSERT(world);
+	m_forceArray = m_jointForce;
+	if (m_maxDOF > 24) {
+		NEWTON_ASSERT(0);
+		m_forceArray = (dgFloat32 *) world->GetAllocator()->Malloc(
+		                   dgInt32(m_maxDOF * sizeof(dgFloat32)));
+	}
+	memset(m_forceArray, 0, m_maxDOF * sizeof(dgFloat32));
+}
+
+NewtonUserJoint::~NewtonUserJoint() {
+	if (m_forceArray != m_jointForce) {
+		m_body0->GetWorld()->GetAllocator()->Free(m_forceArray);
+	}
+
+}
+
+dgUnsigned32 NewtonUserJoint::JacobianDerivative(dgContraintDescritor &params) {
+	m_rows = 0;
+	m_param = &params;
+	m_jacobianFnt((NewtonJoint *) this, params.m_timestep, params.m_threadIndex);
+	return dgUnsigned32(m_rows);
+}
+
+void NewtonUserJoint::AddLinearRowJacobian(const dgVector &pivot0,
+        const dgVector &pivot1, const dgVector &dir) {
+	dgPointParam pointData;
+	InitPointParam(pointData, m_stiffness, pivot0, pivot1);
+
+	m_lastPosit0 = pivot0;
+	m_lastPosit1 = pivot1;
+	m_lastJointAngle = dgFloat32(0.0f);
+	CalculatePointDerivative(m_rows, *m_param, dir, pointData,
+	                         &m_forceArray[m_rows]);
+	m_rows++;
+	NEWTON_ASSERT(m_rows <= dgInt32(m_maxDOF));
+}
+
+void NewtonUserJoint::AddAngularRowJacobian(const dgVector &dir,
+        dgFloat32 relAngle) {
+	m_lastPosit0 = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
+	                        dgFloat32(0.0f));
+	m_lastPosit1 = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
+	                        dgFloat32(0.0f));
+	m_lastJointAngle = relAngle;
+	CalculateAngularDerivative(m_rows, *m_param, dir, m_stiffness, relAngle,
+	                           &m_forceArray[m_rows]);
+	m_rows++;
+	NEWTON_ASSERT(m_rows <= dgInt32(m_maxDOF));
+}
+
+void NewtonUserJoint::AddGeneralRowJacobian(const dgFloat32 *jacobian0,
+        const dgFloat32 *jacobian1) {
+	m_lastPosit0 = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
+	                        dgFloat32(0.0f));
+	m_lastPosit1 = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
+	                        dgFloat32(0.0f));
+	m_lastJointAngle = 0.0f;
+
+	SetJacobianDerivative(m_rows, *m_param, jacobian0, jacobian1,
+	                      &m_forceArray[m_rows]);
+	m_rows++;
+	NEWTON_ASSERT(m_rows <= dgInt32(m_maxDOF));
+}
+
+void NewtonUserJoint::SetAcceleration(dgFloat32 acceleration) {
+	dgInt32 index;
+	index = m_rows - 1;
+	if ((index >= 0) && (index < dgInt32(m_maxDOF))) {
+//		m_param->m_jointAccel[index] = acceleration;
+		SetMotorAcceleration(index, acceleration, *m_param);
+	}
+}
+
+void NewtonUserJoint::SetSpringDamperAcceleration(dFloat springK,
+        dFloat springD) {
+	dgInt32 index;
+	index = m_rows - 1;
+	if ((index >= 0) && (index < dgInt32(m_maxDOF))) {
+		dgFloat32 accel;
+		accel = CalculateSpringDamperAcceleration(index, *m_param, m_lastJointAngle,
+		        m_lastPosit0, m_lastPosit1, springK, springD);
+		NEWTON_ASSERT(0);
+//		m_param->m_jointAccel[index] = accel;
+		SetMotorAcceleration(index, accel, *m_param);
+	}
+}
+
+void NewtonUserJoint::SetHighFriction(dgFloat32 friction) {
+	dgInt32 index;
+	index = m_rows - 1;
+	if ((index >= 0) && (index < dgInt32(m_maxDOF))) {
+		m_param->m_forceBounds[index].m_upper = ClampValue(friction,
+		                                        dgFloat32(0.001f), dgFloat32(DG_MAX_BOUND));
+		m_param->m_forceBounds[index].m_normalIndex =
+		    DG_BILATERAL_FRICTION_CONSTRAINT;
+	}
+}
+
+void NewtonUserJoint::SetLowerFriction(dgFloat32 friction) {
+	dgInt32 index;
+	index = m_rows - 1;
+	if ((index >= 0) && (index < dgInt32(m_maxDOF))) {
+		m_param->m_forceBounds[index].m_low = ClampValue(friction,
+		                                      dgFloat32(DG_MIN_BOUND), dgFloat32(-0.001f));
+		m_param->m_forceBounds[index].m_normalIndex =
+		    DG_BILATERAL_FRICTION_CONSTRAINT;
+	}
+}
+
+void NewtonUserJoint::SetRowStiffness(dgFloat32 stiffness) {
+	dgInt32 index;
+	index = m_rows - 1;
+	if ((index >= 0) && (index < dgInt32(m_maxDOF))) {
+		stiffness = ClampValue(stiffness, dgFloat32(0.0f), dgFloat32(1.0f));
+		stiffness = 100.0f - stiffness * 99.0f;
+		m_param->m_jointStiffness[index] = stiffness;
+	}
+}
+
+dgFloat32 NewtonUserJoint::GetRowForce(dgInt32 row) const {
+	dgFloat32 force;
+
+	force = 0.0f;
+	if ((row >= 0) && (row < dgInt32(m_maxDOF))) {
+		force = m_forceArray[row];
+	}
+	return force;
+}
+
+void NewtonUserJoint::GetInfo(dgConstraintInfo *const info) const {
+	info->clear();
+	if (m_getInfoCallback) {
+		InitInfo(info);
+		m_getInfoCallback((const NewtonJoint *)this, (NewtonJointRecord *) info);
+	}
+}
+
+void NewtonUserJoint::SetUpdateFeedbackFunction(NewtonUserBilateralCallBack getFeedback) {
+	dgUserConstraint::SetUpdateFeedbackFunction((ConstraintsForceFeedback)getFeedback);
+}