Initial commit

engines/hpl1/engine/libraries/newton/physics/dgUpVectorConstraint.cpp

@@ -0,0 +1,136 @@
+/* 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 "dgUpVectorConstraint.h"
+#include "dgBody.h"
+#include "dgWorld.h"
+#include "hpl1/engine/libraries/newton/core/dg.h"
+
+
+//////////////////////////////////////////////////////////////////////
+// Construction/Destruction
+//////////////////////////////////////////////////////////////////////
+
+dgUpVectorConstraint::dgUpVectorConstraint() : dgBilateralConstraint() {
+	NEWTON_ASSERT((sizeof(dgUpVectorConstraint) & 15) == 0);
+	NEWTON_ASSERT((((dgUnsigned64)&m_localMatrix0) & 15) == 0);
+
+	//  dgUpVectorConstraintArray& array = * world;
+	//  constraint = array.GetElement();
+
+	SetStiffness(dgFloat32(0.995f));
+	m_maxDOF = 2;
+	m_constId = dgUpVectorConstraintId;
+	m_callBack = NULL;
+}
+
+dgUpVectorConstraint::~dgUpVectorConstraint() {
+}
+
+/*
+ dgUpVectorConstraint* dgUpVectorConstraint::Create(dgWorld* world)
+ {
+ dgUpVectorConstraint* constraint;
+ // constraint = dgUpVectorConstraintArray::GetPool().GetElement();
+
+ dgUpVectorConstraintArray& array = * world;
+ constraint = array.GetElement();
+
+ NEWTON_ASSERT ((((dgUnsigned64) &constraint->m_localMatrix0) & 15) == 0);
+
+ constraint->Init ();
+
+
+ constraint->SetStiffness (dgFloat32 (0.995f));
+ constraint->m_maxDOF = 2;
+ constraint->m_constId = dgUpVectorConstraintId;
+ constraint->m_callBack = NULL;
+ return constraint;
+ }
+
+ void dgUpVectorConstraint::Remove(dgWorld* world)
+ {
+ dgUpVectorConstraintArray& array = * world;
+ dgBilateralConstraint::Remove (world);
+ // dgUpVectorConstraintArray::GetPool().RemoveElement (this);
+ array.RemoveElement (this);
+ }
+ */
+
+bool dgUpVectorConstraint::IsBilateral() const {
+	return false;
+}
+
+void dgUpVectorConstraint::InitPinDir(const dgVector &pin) {
+
+	const dgMatrix &matrix = m_body0->GetMatrix();
+
+	dgVector pivot(matrix.m_posit);
+	SetPivotAndPinDir(pivot, pin);
+}
+
+void dgUpVectorConstraint::SetPinDir(const dgVector &pin) {
+	m_localMatrix1 = dgMatrix(pin);
+}
+
+dgVector dgUpVectorConstraint::GetPinDir() const {
+	return m_localMatrix1.m_front;
+}
+
+void dgUpVectorConstraint::SetJointParameterCallBack(
+    dgUpVectorJointCallBack callback) {
+	m_callBack = callback;
+}
+
+dgUnsigned32 dgUpVectorConstraint::JacobianDerivative(
+    dgContraintDescritor &params) {
+	dgInt32 ret;
+	dgFloat32 mag;
+	dgFloat32 angle;
+	dgMatrix matrix0;
+	dgMatrix matrix1;
+
+	CalculateGlobalMatrixAndAngle(matrix0, matrix1);
+
+	dgVector lateralDir(matrix0.m_front * matrix1.m_front);
+
+	ret = 0;
+	mag = lateralDir % lateralDir;
+	if (mag > 1.0e-6f) {
+		mag = dgSqrt(mag);
+		lateralDir = lateralDir.Scale(dgFloat32(1.0f) / mag);
+		angle = dgAsin(mag);
+		CalculateAngularDerivative(0, params, lateralDir, m_stiffness, angle,
+		                           &m_jointForce[0]);
+
+		dgVector frontDir(lateralDir * matrix1.m_front);
+		CalculateAngularDerivative(1, params, frontDir, m_stiffness,
+		                           dgFloat32(0.0f), &m_jointForce[1]);
+		ret = 2;
+	} else {
+		CalculateAngularDerivative(0, params, matrix0.m_up, m_stiffness, 0.0,
+		                           &m_jointForce[0]);
+		CalculateAngularDerivative(1, params, matrix0.m_right, m_stiffness,
+		                           dgFloat32(0.0f), &m_jointForce[1]);
+		ret = 2;
+	}
+	return dgUnsigned32(ret);
+}