137 lines
4.0 KiB
C++
137 lines
4.0 KiB
C++
/* 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 ¶ms) {
|
|
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);
|
|
}
|