292 lines
9.4 KiB
C++
292 lines
9.4 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 "dgUniversalConstraint.h"
|
|
#include "dgBody.h"
|
|
#include "dgWorld.h"
|
|
#include "hpl1/engine/libraries/newton/core/dg.h"
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
// Construction/Destruction
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
dgUniversalConstraint::dgUniversalConstraint() : dgBilateralConstraint() {
|
|
NEWTON_ASSERT((((dgUnsigned64)&m_localMatrix0) & 15) == 0);
|
|
m_maxDOF = 6;
|
|
m_constId = dgUniversalConstraintId;
|
|
|
|
m_angle0 = dgFloat32(0.0f);
|
|
m_angle1 = dgFloat32(0.0f);
|
|
m_jointAccelFnt = NULL;
|
|
}
|
|
|
|
dgUniversalConstraint::~dgUniversalConstraint() {
|
|
}
|
|
|
|
/*
|
|
dgUniversalConstraint* dgUniversalConstraint::Create(dgWorld* world)
|
|
{
|
|
dgUniversalConstraint* constraint;
|
|
|
|
// constraint = dgUniversalConstraintArray::GetPool().GetElement();
|
|
|
|
dgUniversalConstraintArray& array = * world;
|
|
constraint = array.GetElement();
|
|
|
|
NEWTON_ASSERT ((((dgUnsigned64) &constraint->m_localMatrix0) & 15) == 0);
|
|
constraint->Init ();
|
|
constraint->m_maxDOF = 6;
|
|
constraint->m_constId = dgUniversalConstraintId;
|
|
|
|
constraint->m_angle0 = dgFloat32 (0.0f);
|
|
constraint->m_angle1 = dgFloat32 (0.0f);
|
|
constraint->m_jointAccelFnt = NULL;
|
|
return constraint;
|
|
}
|
|
|
|
void dgUniversalConstraint::Remove(dgWorld* world)
|
|
{
|
|
dgUniversalConstraintArray& array = * world;
|
|
|
|
dgBilateralConstraint::Remove (world);
|
|
// dgUniversalConstraintArray::GetPool().RemoveElement (this);
|
|
array.RemoveElement (this);
|
|
}
|
|
*/
|
|
|
|
void dgUniversalConstraint::SetJointParameterCallBack(
|
|
dgUniversalJointAcceleration callback) {
|
|
m_jointAccelFnt = callback;
|
|
}
|
|
|
|
dgFloat32 dgUniversalConstraint::GetJointAngle0() const {
|
|
return m_angle0;
|
|
}
|
|
|
|
dgFloat32 dgUniversalConstraint::GetJointAngle1() const {
|
|
return m_angle1;
|
|
}
|
|
|
|
dgFloat32 dgUniversalConstraint::GetJointOmega0() const {
|
|
NEWTON_ASSERT(m_body0);
|
|
NEWTON_ASSERT(m_body1);
|
|
|
|
dgVector dir(m_body0->GetMatrix().RotateVector(m_localMatrix0[0]));
|
|
const dgVector &omega0 = m_body0->GetOmega();
|
|
const dgVector &omega1 = m_body1->GetOmega();
|
|
|
|
// dgVector omega1 (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
|
|
// if (m_body1) {
|
|
// omega1 = m_body1->GetOmega();
|
|
// }
|
|
return (omega0 - omega1) % dir;
|
|
}
|
|
|
|
dgFloat32 dgUniversalConstraint::GetJointOmega1() const {
|
|
NEWTON_ASSERT(m_body0);
|
|
NEWTON_ASSERT(m_body1);
|
|
|
|
dgVector dir(m_body1->GetMatrix().RotateVector(m_localMatrix1[1]));
|
|
const dgVector &omega0 = m_body0->GetOmega();
|
|
const dgVector &omega1 = m_body1->GetOmega();
|
|
|
|
// dgMatrix matrix (dgGetIdentityMatrix());
|
|
// if (m_body1) {
|
|
// matrix = m_body1->GetMatrix();
|
|
// omega1 = m_body1->GetOmega();
|
|
// }
|
|
// dgVector dir (matrix.RotateVector (m_localMatrix1[1]));
|
|
|
|
return (omega0 - omega1) % dir;
|
|
}
|
|
|
|
dgFloat32 dgUniversalConstraint::CalculateStopAlpha0(dgFloat32 angle,
|
|
const dgJointCallBackParam *param) const {
|
|
dgFloat32 alpha;
|
|
dgFloat32 omega;
|
|
dgFloat32 penetrationErr;
|
|
|
|
alpha = dgFloat32(0.0f);
|
|
if (m_angle0 > angle) {
|
|
omega = GetJointOmega0();
|
|
if (omega < dgFloat32(0.0f)) {
|
|
omega = dgFloat32(0.0f);
|
|
}
|
|
penetrationErr = angle - m_angle0;
|
|
alpha = 100.0f * penetrationErr - omega * 1.01f / param->m_timestep;
|
|
|
|
} else if (m_angle0 < angle) {
|
|
omega = GetJointOmega0();
|
|
if (omega > dgFloat32(0.0f)) {
|
|
omega = dgFloat32(0.0f);
|
|
}
|
|
|
|
penetrationErr = angle - m_angle0;
|
|
alpha = 100.0f * penetrationErr - omega * 1.01f / param->m_timestep;
|
|
}
|
|
return alpha;
|
|
}
|
|
|
|
dgFloat32 dgUniversalConstraint::CalculateStopAlpha1(dgFloat32 angle,
|
|
const dgJointCallBackParam *param) const {
|
|
dgFloat32 alpha;
|
|
dgFloat32 omega;
|
|
dgFloat32 penetrationErr;
|
|
|
|
alpha = dgFloat32(0.0f);
|
|
if (m_angle1 > angle) {
|
|
omega = GetJointOmega1();
|
|
if (omega > dgFloat32(0.0f)) {
|
|
omega = dgFloat32(0.0f);
|
|
}
|
|
penetrationErr = m_angle1 - angle;
|
|
// alpha = (penetrationErr + omega * 1.01f) / param->m_timestep;
|
|
alpha = penetrationErr * 100.0f - omega * 1.01f / param->m_timestep;
|
|
} else if (m_angle1 < angle) {
|
|
omega = GetJointOmega1();
|
|
if (omega < dgFloat32(0.0f)) {
|
|
omega = dgFloat32(0.0f);
|
|
}
|
|
|
|
penetrationErr = m_angle1 - angle;
|
|
// alpha = (penetrationErr + omega * 1.01f) / param->m_timestep;
|
|
alpha = penetrationErr * 100.0f - omega * 1.01f / param->m_timestep;
|
|
}
|
|
return alpha;
|
|
}
|
|
|
|
dgVector dgUniversalConstraint::GetJointForce() const {
|
|
dgMatrix matrix0;
|
|
dgMatrix matrix1;
|
|
|
|
CalculateGlobalMatrixAndAngle(matrix0, matrix1);
|
|
|
|
return dgVector(
|
|
matrix0.m_up.Scale(m_jointForce[0]) + matrix0.m_right.Scale(m_jointForce[1]) + matrix0.m_up.Scale(m_jointForce[2]) + matrix0.m_right.Scale(m_jointForce[3]));
|
|
}
|
|
|
|
dgUnsigned32 dgUniversalConstraint::JacobianDerivative(
|
|
dgContraintDescritor ¶ms) {
|
|
dgInt32 ret;
|
|
dgFloat32 sinAngle;
|
|
dgFloat32 cosAngle;
|
|
dgMatrix matrix0;
|
|
dgMatrix matrix1;
|
|
|
|
CalculateGlobalMatrixAndAngle(matrix0, matrix1);
|
|
|
|
const dgVector &dir0 = matrix0.m_front;
|
|
const dgVector &dir1 = matrix1.m_up;
|
|
dgVector dir2(dir0 * dir1);
|
|
|
|
dgVector dir3(dir2 * dir0);
|
|
dir3 = dir3.Scale(dgRsqrt(dir3 % dir3));
|
|
|
|
const dgVector &p0 = matrix0.m_posit;
|
|
const dgVector &p1 = matrix1.m_posit;
|
|
|
|
dgVector q0(p0 + dir3.Scale(MIN_JOINT_PIN_LENGTH));
|
|
dgVector q1(p1 + dir1.Scale(MIN_JOINT_PIN_LENGTH));
|
|
|
|
dgPointParam pointDataP;
|
|
dgPointParam pointDataQ;
|
|
InitPointParam(pointDataP, m_stiffness, p0, p1);
|
|
InitPointParam(pointDataQ, m_stiffness, q0, q1);
|
|
|
|
CalculatePointDerivative(0, params, dir0, pointDataP, &m_jointForce[0]);
|
|
CalculatePointDerivative(1, params, dir1, pointDataP, &m_jointForce[1]);
|
|
CalculatePointDerivative(2, params, dir2, pointDataP, &m_jointForce[2]);
|
|
CalculatePointDerivative(3, params, dir0, pointDataQ, &m_jointForce[3]);
|
|
ret = 4;
|
|
|
|
// dgVector sinAngle0 (matrix1.m_up * matrix0.m_up);
|
|
// m_angle0 = dgAsin (ClampValue (sinAngle0 % dir0, -0.9999999f, 0.9999999f));
|
|
// if ((matrix0.m_up % matrix1.m_up) < dgFloat32 (0.0f)) {
|
|
// m_angle0 = (m_angle0 >= dgFloat32 (0.0f)) ? dgPI - m_angle0 : dgPI + m_angle0;
|
|
// }
|
|
|
|
sinAngle = (matrix1.m_up * matrix0.m_up) % matrix0.m_front;
|
|
cosAngle = matrix0.m_up % matrix1.m_up;
|
|
// NEWTON_ASSERT (dgAbsf (m_angle0 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
|
|
m_angle0 = dgAtan2(sinAngle, cosAngle);
|
|
|
|
// dgVector sinAngle1 (matrix0.m_front * matrix1.m_front);
|
|
// m_angle1 = dgAsin (ClampValue (sinAngle1 % dir1, -0.9999999f, 0.9999999f));
|
|
// if ((matrix0.m_front % matrix1.m_front) < dgFloat32 (0.0f)) {
|
|
// m_angle1 = (m_angle1 >= dgFloat32 (0.0f)) ? dgPI - m_angle1 : dgPI + m_angle1;
|
|
// }
|
|
|
|
sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up;
|
|
cosAngle = matrix0.m_front % matrix1.m_front;
|
|
// NEWTON_ASSERT (dgAbsf (m_angle1 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
|
|
m_angle1 = dgAtan2(sinAngle, cosAngle);
|
|
|
|
if (m_jointAccelFnt) {
|
|
dgUnsigned32 code;
|
|
dgJointCallBackParam axisParam[2];
|
|
|
|
// linear acceleration
|
|
axisParam[0].m_accel = dgFloat32(0.0f);
|
|
axisParam[0].m_timestep = params.m_timestep;
|
|
axisParam[0].m_minFriction = DG_MIN_BOUND;
|
|
axisParam[0].m_maxFriction = DG_MAX_BOUND;
|
|
|
|
// angular acceleration
|
|
axisParam[1].m_accel = dgFloat32(0.0f);
|
|
axisParam[1].m_timestep = params.m_timestep;
|
|
axisParam[1].m_minFriction = DG_MIN_BOUND;
|
|
axisParam[1].m_maxFriction = DG_MAX_BOUND;
|
|
|
|
code = m_jointAccelFnt(reinterpret_cast<NewtonJoint *>(this), reinterpret_cast<NewtonHingeSliderUpdateDesc *>(axisParam));
|
|
if (code & 1) {
|
|
if ((axisParam[0].m_minFriction > DG_MIN_BOUND) || (axisParam[0].m_maxFriction < DG_MAX_BOUND)) {
|
|
params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction;
|
|
params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction;
|
|
params.m_forceBounds[ret].m_normalIndex =
|
|
DG_BILATERAL_FRICTION_CONSTRAINT;
|
|
}
|
|
|
|
// CalculatePointDerivative (ret, params, dir0, pointDataP, &m_jointForce[ret]);
|
|
CalculateAngularDerivative(ret, params, dir0, m_stiffness,
|
|
dgFloat32(0.0f), &m_jointForce[ret]);
|
|
// params.m_jointAccel[ret] = axisParam[0].m_accel;
|
|
SetMotorAcceleration(ret, axisParam[0].m_accel, params);
|
|
ret++;
|
|
}
|
|
|
|
if (code & 2) {
|
|
if ((axisParam[1].m_minFriction > DG_MIN_BOUND) || (axisParam[1].m_maxFriction < DG_MAX_BOUND)) {
|
|
params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction;
|
|
params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction;
|
|
params.m_forceBounds[ret].m_normalIndex =
|
|
DG_BILATERAL_FRICTION_CONSTRAINT;
|
|
}
|
|
CalculateAngularDerivative(ret, params, dir1, m_stiffness,
|
|
dgFloat32(0.0f), &m_jointForce[ret]);
|
|
// params.m_jointAccel[ret] = axisParam[1].m_accel;
|
|
SetMotorAcceleration(ret, axisParam[1].m_accel, params);
|
|
ret++;
|
|
}
|
|
}
|
|
return dgUnsigned32(ret);
|
|
}
|