void CustomSlider::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
dVector p0(matrix0.m_posit);
dVector p1(matrix1.m_posit + matrix1.m_front.Scale((p0 - matrix1.m_posit) % matrix1.m_front));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix1.m_right[0]);
// three rows to restrict rotation around around the parent coordinate system
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front), &matrix1.m_front[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]);
// calculate position and speed
dVector veloc0(0.0f);
dVector veloc1(0.0f);
dAssert (m_body0);
NewtonBodyGetPointVelocity(m_body0, &matrix0.m_posit[0], &veloc0[0]);
if (m_body1) {
NewtonBodyGetPointVelocity(m_body1, &matrix1.m_posit[0], &veloc1[0]);
}
m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix1.m_front;
m_speed = (veloc0 - veloc1) % matrix1.m_front;
m_lastRowWasUsed = false;
SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
}
void dCustomTireSpringDG::SubmitConstraints(dFloat timestep, int threadIndex)
{
NewtonBody* BodyAttach;
//NewtonBody* BodyFrame;
//
dVector tireOmega = dVector(0.0f, 0.0f, 0.0f);
//BodyFrame = GetBody0();
BodyAttach = GetBody1();
//
SteeringController(timestep);
//
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(mChassisPivotMatrix, mTirePivotMatrix);
//
NewtonBodyGetOmega(BodyAttach, &tireOmega[0]);
//
mRealOmega = dAbs(tireOmega.DotProduct3(mChassisPivotMatrix.m_front));
//
TireCenterPin(timestep);
//
TireCenterBolt(timestep);
//
SuspenssionSpringLimits(timestep);
//
TireBreakAction(BodyAttach, timestep);
}
void CustomPulley::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
dVector veloc0;
dVector veloc1;
dFloat jacobian0[6];
dFloat jacobian1[6];
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// calculate the angular velocity for both bodies
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
// get angular velocity relative to the pin vector
dFloat w0 = veloc0 % matrix0.m_front;
dFloat w1 = veloc1 % matrix1.m_front;
// establish the gear equation.
dFloat relVeloc = w0 + m_gearRatio * w1;
if (m_gearRatio > dFloat(1.0f)) {
relVeloc = w0 / m_gearRatio + w1;
}
// calculate the relative angular acceleration by dividing by the time step
// ideally relative acceleration should be zero, but is practice there will always
// be a small difference in velocity that need to be compensated.
// using the full acceleration will make the to over show a oscillate
// so use only fraction of the acceleration
dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep: 1.0f;
dFloat relAccel = - 0.3f * relVeloc * invTimestep;
// set the linear part of Jacobian 0 to translational pin vector
jacobian0[0] = matrix0.m_front[0];
jacobian0[1] = matrix0.m_front[1];
jacobian0[2] = matrix0.m_front[2];
// set the rotational part of Jacobian 0 to zero
jacobian0[3] = 0.0f;
jacobian0[4] = 0.0f;
jacobian0[5] = 0.0f;
// set the linear part of Jacobian 1 to translational pin vector
jacobian1[0] = matrix1.m_front[0];
jacobian1[1] = matrix1.m_front[1];
jacobian1[2] = matrix1.m_front[2];
// set the rotational part of Jacobian 1 to zero
jacobian1[3] = 0.0f;
jacobian1[4] = 0.0f;
jacobian1[5] = 0.0f;
// add a angular constraint
NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);
// set the desired angular acceleration between the two bodies
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
void dCustomUpVector::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// if the body ha rotated by some amount, the there will be a plane of rotation
dVector lateralDir (matrix0.m_front.CrossProduct(matrix1.m_front));
dFloat mag = lateralDir.DotProduct3(lateralDir);
if (mag > 1.0e-6f) {
// if the side vector is not zero, it means the body has rotated
mag = dSqrt (mag);
lateralDir = lateralDir.Scale (1.0f / mag);
dFloat angle = dAsin (mag);
// add an angular constraint to correct the error angle
NewtonUserJointAddAngularRow (m_joint, angle, &lateralDir[0]);
// in theory only one correction is needed, but this produces instability as the body may move sideway.
// a lateral correction prevent this from happening.
dVector frontDir (lateralDir.CrossProduct(matrix1.m_front));
NewtonUserJointAddAngularRow (m_joint, 0.0f, &frontDir[0]);
} else {
// if the angle error is very small then two angular correction along the plane axis do the trick
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
}
}
int DP_GlobalBlockAlign(
const DP_BlockInfo *blocks, DP_BlockScoreFunction BlockScore,
unsigned int queryFrom, unsigned int queryTo,
DP_AlignmentResult **alignment)
{
if (!blocks || blocks->nBlocks < 1 || !blocks->blockSizes || !BlockScore || queryTo < queryFrom) {
ERROR_MESSAGE("DP_GlobalBlockAlign() - invalid parameters");
return STRUCT_DP_PARAMETER_ERROR;
}
unsigned int i, sumBlockLen = 0;
for (i=0; i<blocks->nBlocks; ++i)
sumBlockLen += blocks->blockSizes[i];
if (sumBlockLen > queryTo - queryFrom + 1) {
ERROR_MESSAGE("DP_GlobalBlockAlign() - sum of block lengths longer than query region");
return STRUCT_DP_PARAMETER_ERROR;
}
int status = ValidateFrozenBlockPositions(blocks, queryFrom, queryTo, true);
if (status != STRUCT_DP_OKAY) {
ERROR_MESSAGE("DP_GlobalBlockAlign() - ValidateFrozenBlockPositions() returned error");
return status;
}
Matrix matrix(blocks->nBlocks, queryTo - queryFrom + 1);
status = CalculateGlobalMatrix(matrix, blocks, BlockScore, queryFrom, queryTo);
if (status != STRUCT_DP_OKAY) {
ERROR_MESSAGE("DP_GlobalBlockAlign() - CalculateGlobalMatrix() failed");
return status;
}
return TracebackGlobalAlignment(matrix, blocks, queryFrom, queryTo, alignment);
}
dVector CustomUniversal::GetPinAxis_1 () const
{
dMatrix matrix0;
dMatrix matrix1;
CalculateGlobalMatrix (matrix0, matrix1);
return matrix1.m_up;
}
void SubmitConstraints(dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(matrix0, matrix1);
dVector p0(matrix0.m_posit);
dVector p1(matrix1.m_posit);
dVector dir(p1 - p0);
dFloat mag2 = dir % dir;
dir = dir.Scale(1.0f / dSqrt(mag2));
dMatrix matrix(dGrammSchmidt(dir));
dFloat x = dSqrt(mag2) - m_distance;
dVector com0;
dVector com1;
dVector veloc0;
dVector veloc1;
dMatrix body0Matrix;
dMatrix body1Matrix;
NewtonBody* const body0 = GetBody0();
NewtonBody* const body1 = GetBody1();
NewtonBodyGetCentreOfMass(body0, &com0[0]);
NewtonBodyGetMatrix(body0, &body0Matrix[0][0]);
NewtonBodyGetPointVelocity(body0, &p0[0], &veloc0[0]);
NewtonBodyGetCentreOfMass(body1, &com1[0]);
NewtonBodyGetMatrix(body1, &body1Matrix[0][0]);
NewtonBodyGetPointVelocity(body1, &p1[0], &veloc1[0]);
dFloat v((veloc0 - veloc1) % dir);
dFloat a = (x - v * timestep) / (timestep * timestep);
dVector r0((p0 - body0Matrix.TransformVector(com0)) * matrix.m_front);
dVector r1((p1 - body1Matrix.TransformVector(com1)) * matrix.m_front);
dFloat jacobian0[6];
dFloat jacobian1[6];
jacobian0[0] = matrix[0][0];
jacobian0[1] = matrix[0][1];
jacobian0[2] = matrix[0][2];
jacobian0[3] = r0[0];
jacobian0[4] = r0[1];
jacobian0[5] = r0[2];
jacobian1[0] = -matrix[0][0];
jacobian1[1] = -matrix[0][1];
jacobian1[2] = -matrix[0][2];
jacobian1[3] = -r1[0];
jacobian1[4] = -r1[1];
jacobian1[5] = -r1[2];
NewtonUserJointAddGeneralRow(m_joint, jacobian0, jacobian1);
NewtonUserJointSetRowAcceleration(m_joint, a);
}
void SubmitConstraints(dFloat timestep, int threadIndex)
{
CustomBallAndSocket::SubmitConstraints(timestep, threadIndex);
float invTimestep = 1.0f / timestep;
dMatrix matrix0;
dMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
if (m_anim_speed != 0.0f) // some animation to illustrate purpose
{
m_anim_time += timestep * m_anim_speed;
float a0 = sin(m_anim_time);
float a1 = m_anim_offset * 3.14f;
dVector axis(sin(a1), 0.0f, cos(a1));
//dVector axis (1,0,0);
m_target = dQuaternion(axis, a0 * 0.5f);
}
// measure error
dQuaternion q0(matrix0);
dQuaternion q1(matrix1);
dQuaternion qt0 = m_target * q1;
dQuaternion qErr = ((q0.DotProduct(qt0) < 0.0f) ? dQuaternion(-q0.m_q0, q0.m_q1, q0.m_q2, q0.m_q3) : dQuaternion(q0.m_q0, -q0.m_q1, -q0.m_q2, -q0.m_q3)) * qt0;
float errorAngle = 2.0f * acos(dMax(-1.0f, dMin(1.0f, qErr.m_q0)));
dVector errorAngVel(0, 0, 0);
dMatrix basis;
if (errorAngle > 1.0e-10f) {
dVector errorAxis(qErr.m_q1, qErr.m_q2, qErr.m_q3, 0.0f);
errorAxis = errorAxis.Scale(1.0f / dSqrt(errorAxis % errorAxis));
errorAngVel = errorAxis.Scale(errorAngle * invTimestep);
basis = dGrammSchmidt(errorAxis);
} else {
basis = dMatrix(qt0, dVector(0.0f, 0.0f, 0.0f, 1.0f));
}
dVector angVel0, angVel1;
NewtonBodyGetOmega(m_body0, (float*)&angVel0);
NewtonBodyGetOmega(m_body1, (float*)&angVel1);
dVector angAcc = (errorAngVel.Scale(m_reduceError) - (angVel0 - angVel1)).Scale(invTimestep);
// motor
for (int n = 0; n < 3; n++) {
// calculate the desired acceleration
dVector &axis = basis[n];
float relAccel = angAcc % axis;
NewtonUserJointAddAngularRow(m_joint, 0.0f, &axis[0]);
NewtonUserJointSetRowAcceleration(m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
}
}
void dCustomJoint::Debug(dDebugDisplay* const debugDisplay) const
{
dMatrix matrix0;
dMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
debugDisplay->DrawFrame(matrix0);
debugDisplay->DrawFrame(matrix1);
}
void CustomCorkScrew::GetInfo (NewtonJointRecord* const info) const
{
strcpy (info->m_descriptionType, "corkScrew");
info->m_attachBody_0 = m_body0;
info->m_attachBody_1 = m_body1;
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
if (m_limitsLinearOn) {
dFloat dist;
dist = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
info->m_minLinearDof[0] = m_minLinearDist - dist;
info->m_maxLinearDof[0] = m_maxLinearDist - dist;
} else {
info->m_minLinearDof[0] = -FLT_MAX ;
info->m_maxLinearDof[0] = FLT_MAX ;
}
info->m_minLinearDof[1] = 0.0f;
info->m_maxLinearDof[1] = 0.0f;;
info->m_minLinearDof[2] = 0.0f;
info->m_maxLinearDof[2] = 0.0f;
// info->m_minAngularDof[0] = -FLT_MAX;
// info->m_maxAngularDof[0] = FLT_MAX;
if (m_limitsAngularOn) {
dFloat angle;
dFloat sinAngle;
dFloat cosAngle;
sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
angle = dAtan2 (sinAngle, cosAngle);
info->m_minAngularDof[0] = (m_minAngularDist - angle) * 180.0f / 3.141592f ;
info->m_maxAngularDof[0] = (m_maxAngularDist - angle) * 180.0f / 3.141592f ;
} else {
info->m_minAngularDof[0] = -FLT_MAX ;
info->m_maxAngularDof[0] = FLT_MAX;
}
info->m_minAngularDof[1] = 0.0f;
info->m_maxAngularDof[1] = 0.0f;
info->m_minAngularDof[2] = 0.0f;
info->m_maxAngularDof[2] = 0.0f;
memcpy (info->m_attachmenMatrix_0, &m_localMatrix0, sizeof (dMatrix));
memcpy (info->m_attachmenMatrix_1, &m_localMatrix1, sizeof (dMatrix));
}
void dCustomTireSpringDG::Debug(dDebugDisplay* const debugDisplay) const
{
dMatrix matrix0;
dMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
debugDisplay->DrawFrame(matrix0);
debugDisplay->DrawFrame(matrix1);
//
if (m_options.m_option0) {
}
//
}
void CustomBallAndSocket::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all three orthonormal directions
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
}
void Custom6DOF::GetInfo (NewtonJointRecord* const info) const
{
dMatrix matrix0;
dMatrix matrix1;
dFloat dist;
strcpy (info->m_descriptionType, GetTypeName());
info->m_attachBody_0 = m_body0;
info->m_attachBody_1 = m_body1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
dVector p0 (matrix0.m_posit);
dVector p1 (matrix1.m_posit);
dVector dp (p0 - p1);
for (int i = 0; i < 3; i ++) {
if (!((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f))) {
p1 += matrix1[i].Scale (dp.DotProduct3(matrix1[i]));
}
}
for (int i = 0; i < 3; i ++) {
if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) {
info->m_minLinearDof[i] = 0.0f;
info->m_maxLinearDof[i] = 0.0f;
} else {
dist = dp.DotProduct3(matrix1[i]);
info->m_maxLinearDof[i] = m_maxLinearLimits[i] - dist;
info->m_minLinearDof[i] = m_minLinearLimits[i] - dist;
}
}
dVector eulerAngles (m_pitch.GetAngle(), m_yaw.GetAngle(), m_roll.GetAngle(), 0.0f);
for (int i = 0; i < 3; i ++) {
if ((m_minAngularLimits[i] == 0.0f) && (m_maxAngularLimits[i] == 0.0f)) {
info->m_minAngularDof[i] = 0.0f;
info->m_maxAngularDof[i] = 0.0f;
} else {
info->m_maxAngularDof[i] = (m_maxAngularLimits[i] - eulerAngles[i]) * 180.0f / 3.141592f;
info->m_minAngularDof[i] = (m_minAngularLimits[i] - eulerAngles[i]) * 180.0f / 3.141592f;
}
}
info->m_bodiesCollisionOn = GetBodiesCollisionState();
memcpy (info->m_attachmenMatrix_0, &m_localMatrix0, sizeof (dMatrix));
memcpy (info->m_attachmenMatrix_1, &m_localMatrix1, sizeof (dMatrix));
}
void CustomConeLimitedBallAndSocket::SubmitConstrainst ()
{
dFloat coneCos;
dMatrix matrix0;
dMatrix matrix1;
// add the tree rows to keep the pivot in place
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
// ///////////////////////////////////////////////////////////////////
//
// add a row to keep the child body inside the cone limit
//
// The child is inside the cone if the dCos of the angle between the pin and
coneCos = matrix0.m_front % matrix1.m_front;
if (coneCos < m_cosConeAngle) {
// the child body has violated the cone limit we need to stop it from keep moving
// for that we are going to pick a point along the the child body front vector
dVector p0 (matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector p1 (matrix1.m_posit + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// get a vectors perpendicular to the plane of motion
dVector lateralDir (matrix0.m_front * matrix1.m_front);
// note this could fail if the angle between matrix0.m_front and matrix1.m_front is 90 degree
dVector unitLateralDir = lateralDir.Scale (1.0f / dSqrt (lateralDir % lateralDir));
// now we will add a constraint row along the lateral direction,
// this will add stability as it will prevent the child body from moving sideways
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &unitLateralDir[0]);
// calculate the unit vector tangent to the trajectory
dVector tangentDir (unitLateralDir * matrix0.m_front);
p1 = p0 + (p1 - p0).Scale (0.3f);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &tangentDir[0]);
//we need to allow the body to mo in opposite direction to the penetration
//that can be done by setting the min friction to zero
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
}
void dCustomPulley::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
dVector veloc0(0.0f);
dVector veloc1(0.0f);
dFloat jacobian0[6];
dFloat jacobian1[6];
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// set the linear part of Jacobian 0 to translational pin vector
dVector dir0 (matrix0.m_front.Scale (1.0f/m_gearRatio));
const dVector& dir1 = matrix1.m_front;
jacobian0[0] = dir0.m_x;
jacobian0[1] = dir0.m_y;
jacobian0[2] = dir0.m_z;
jacobian0[3] = 0.0f;
jacobian0[4] = 0.0f;
jacobian0[5] = 0.0f;
jacobian1[0] = dir1.m_x;
jacobian1[1] = dir1.m_y;
jacobian1[2] = dir1.m_z;
jacobian1[3] = 0.0f;
jacobian1[4] = 0.0f;
jacobian1[5] = 0.0f;
// calculate the angular velocity for both bodies
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
// get angular velocity relative to the pin vector
dFloat w0 = veloc0.DotProduct3(dir0);
dFloat w1 = veloc1.DotProduct3(dir1);
dFloat relVeloc = w0 + w1;
dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
dFloat relAccel = -0.5f * relVeloc * invTimestep;
// add a angular constraint
NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);
// set the desired angular acceleration between the two bodies
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
void CustomHinge::GetInfo (NewtonJointRecord* info) const
{
strcpy (info->m_descriptionType, "hinge");
info->m_attachBody_0 = m_body0;
info->m_attachBody_1 = m_body1;
info->m_minLinearDof[0] = 0.0f;
info->m_maxLinearDof[0] = 0.0f;
info->m_minLinearDof[1] = 0.0f;
info->m_maxLinearDof[1] = 0.0f;;
info->m_minLinearDof[2] = 0.0f;
info->m_maxLinearDof[2] = 0.0f;
// the joint angle can be determine by getting the angle between any two non parallel vectors
if (m_limitsOn) {
dFloat angle;
dFloat sinAngle;
dFloat cosAngle;
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
angle = dAtan2 (sinAngle, cosAngle);
info->m_minAngularDof[0] = (m_minAngle - angle) * 180.0f / 3.141592f ;
info->m_maxAngularDof[0] = (m_maxAngle - angle) * 180.0f / 3.141592f ;
} else {
info->m_minAngularDof[0] = -FLT_MAX ;
info->m_maxAngularDof[0] = FLT_MAX ;
}
info->m_minAngularDof[1] = 0.0f;
info->m_maxAngularDof[1] = 0.0f;
info->m_minAngularDof[2] = 0.0f;
info->m_maxAngularDof[2] = 0.0f;
memcpy (info->m_attachmenMatrix_0, &m_localMatrix0, sizeof (dMatrix));
memcpy (info->m_attachmenMatrix_1, &m_localMatrix1, sizeof (dMatrix));
}
void dCustomHinge::SubmitConstraints(dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
dFloat sinAngle;
dFloat cosAngle;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(matrix0, matrix1);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
// two rows to restrict rotation around around the parent coordinate system
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
// the joint angle can be determined by getting the angle between any two non parallel vectors
CalculateAngle (matrix1.m_up, matrix0.m_up, matrix1.m_front, sinAngle, cosAngle);
m_curJointAngle.Update(cosAngle, sinAngle);
// save the current joint Omega
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
m_jointOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);
m_lastRowWasUsed = false;
if (m_setAsSpringDamper) {
ApplySpringDamper (timestep, matrix0, matrix1);
} else {
SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
}
}
void CustomSlider::GetInfo(NewtonJointRecord* const info) const
{
strcpy(info->m_descriptionType, "slider");
info->m_attachBody_0 = m_body0;
info->m_attachBody_1 = m_body1;
if (m_limitsOn) {
dFloat dist;
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(matrix0, matrix1);
dist = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
info->m_minLinearDof[0] = m_minDist - dist;
info->m_maxLinearDof[0] = m_maxDist - dist;
}
else {
info->m_minLinearDof[0] = -D_CUSTOM_LARGE_VALUE;
info->m_maxLinearDof[0] = D_CUSTOM_LARGE_VALUE;
}
info->m_minLinearDof[1] = 0.0f;
info->m_maxLinearDof[1] = 0.0f;;
info->m_minLinearDof[2] = 0.0f;
info->m_maxLinearDof[2] = 0.0f;
info->m_minAngularDof[0] = 0.0f;
info->m_maxAngularDof[0] = 0.0f;
info->m_minAngularDof[1] = 0.0f;
info->m_maxAngularDof[1] = 0.0f;
info->m_minAngularDof[2] = 0.0f;
info->m_maxAngularDof[2] = 0.0f;
memcpy(info->m_attachmenMatrix_0, &m_localMatrix0, sizeof (dMatrix));
memcpy(info->m_attachmenMatrix_1, &m_localMatrix1, sizeof (dMatrix));
}
void CustomUniversalActuator::SubmitConstraints (dFloat timestep, int threadIndex)
{
CustomUniversal::SubmitConstraints (timestep, threadIndex);
if (m_flag0 | m_flag1){
dMatrix matrix0;
dMatrix matrix1;
CalculateGlobalMatrix (matrix0, matrix1);
if (m_flag0) {
dFloat jointAngle = GetJointAngle_0();
dFloat relAngle = jointAngle - m_angle0;
NewtonUserJointAddAngularRow (m_joint, -relAngle, &matrix0.m_front[0]);
dFloat step = m_angularRate0 * timestep;
if (dAbs (relAngle) > 2.0f * dAbs (step)) {
dFloat desiredSpeed = dSign(relAngle) * m_angularRate0;
dFloat currentSpeed = GetJointOmega_0 ();
dFloat accel = (desiredSpeed - currentSpeed) / timestep;
NewtonUserJointSetRowAcceleration (m_joint, accel);
}
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce0);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxForce0);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
}
if (m_flag1) {
dFloat jointAngle = GetJointAngle_1();
dFloat relAngle = jointAngle - m_angle1;
NewtonUserJointAddAngularRow (m_joint, -relAngle, &matrix1.m_up[0]);
dFloat step = m_angularRate1 * timestep;
if (dAbs (relAngle) > 2.0f * dAbs (step)) {
dFloat desiredSpeed = dSign(relAngle) * m_angularRate1;
dFloat currentSpeed = GetJointOmega_1 ();
dFloat accel = (desiredSpeed - currentSpeed) / timestep;
NewtonUserJointSetRowAcceleration (m_joint, accel);
}
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce1);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxForce1);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
}
}
}
void CustomHinge::SubmitConstrainst ()
{
dFloat angle;
dFloat sinAngle;
dFloat cosAngle;
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the jacobian direction vectors, in global space.
CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
// Restrict the movemenet on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
// get a point along the pin axis at some resonable large distance from the pivot
dVector q0 (matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1 (matrix1.m_posit + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// two contraints row perpendicular to the pin vector
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]);
// if limit are enable ...
if (m_limitsOn) {
// the joint angle can be determine by getting the angle between any two non parallel vectors
sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
angle = atan2f (sinAngle, cosAngle);
if (angle < m_minAngle) {
// get a point along the up vector and set a constraint
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowMaximunFriction (m_joint, 0.0f);
} else if (angle > m_maxAngle) {
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimunFriction (m_joint, 0.0f);
}
}
}
void dCustomCorkScrew::Debug(dDebugDisplay* const debugDisplay) const
{
dCustomSlider::Debug(debugDisplay);
if (m_options.m_option2) {
dMatrix matrix0;
dMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
const int subdiv = 12;
dVector arch[subdiv + 1];
const dFloat radius = debugDisplay->m_debugScale;
if ((m_maxAngle > 1.0e-3f) || (m_minAngle < -1.0e-3f)) {
// show pitch angle limits
dVector point(dFloat(0.0f), dFloat(radius), dFloat(0.0f), dFloat(0.0f));
dFloat minAngle = m_minAngle;
dFloat maxAngle = m_maxAngle;
if ((maxAngle - minAngle) >= dPi * 2.0f) {
minAngle = 0.0f;
maxAngle = dPi * 2.0f;
}
dFloat angleStep = (maxAngle - minAngle) / subdiv;
dFloat angle0 = minAngle;
matrix1.m_posit = matrix0.m_posit;
debugDisplay->SetColor(dVector(0.5f, 0.0f, 0.0f, 0.0f));
for (int i = 0; i <= subdiv; i++) {
arch[i] = matrix1.TransformVector(dPitchMatrix(angle0).RotateVector(point));
debugDisplay->DrawLine(matrix1.m_posit, arch[i]);
angle0 += angleStep;
}
for (int i = 0; i < subdiv; i++) {
debugDisplay->DrawLine(arch[i], arch[i + 1]);
}
}
}
}
void dCustomRackAndPinion::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
dVector omega0(0.0f);
dVector veloc1(0.0f);
dFloat jacobian0[6];
dFloat jacobian1[6];
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// calculate the angular velocity for both bodies
NewtonBodyGetOmega(m_body0, &omega0[0]);
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
dVector dir0 (matrix0.m_front.Scale (m_gearRatio));
const dVector& dir1 = matrix1.m_front;
jacobian0[0] = dFloat(0.0f);
jacobian0[1] = dFloat(0.0f);
jacobian0[2] = dFloat(0.0f);
jacobian0[3] = dir0.m_x;
jacobian0[4] = dir0.m_y;
jacobian0[5] = dir0.m_z;
jacobian1[0] = dir1.m_x;
jacobian1[1] = dir1.m_y;
jacobian1[2] = dir1.m_z;
jacobian1[3] = dFloat(0.0f);
jacobian1[4] = dFloat(0.0f);
jacobian1[5] = dFloat(0.0f);
dFloat w0 = omega0.DotProduct3(dir0);
dFloat w1 = veloc1.DotProduct3(dir1);
dFloat relOmega = w0 + w1;
dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
dFloat relAccel = -0.5f * relOmega * invTimestep;
NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
void CustomHinge::GetInfo (NewtonJointRecord* const info) const
{
strcpy (info->m_descriptionType, GetTypeName());
info->m_attachBody_0 = m_body0;
info->m_attachBody_1 = m_body1;
info->m_minLinearDof[0] = 0.0f;
info->m_maxLinearDof[0] = 0.0f;
info->m_minLinearDof[1] = 0.0f;
info->m_maxLinearDof[1] = 0.0f;;
info->m_minLinearDof[2] = 0.0f;
info->m_maxLinearDof[2] = 0.0f;
// the joint angle can be determine by getting the angle between any two non parallel vectors
if (m_limitsOn) {
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
dFloat angle = m_curJointAngle.GetAngle();
info->m_minAngularDof[0] = (m_minAngle - angle) * 180.0f / 3.141592f ;
info->m_maxAngularDof[0] = (m_maxAngle - angle) * 180.0f / 3.141592f ;
} else {
info->m_minAngularDof[0] = -D_CUSTOM_LARGE_VALUE ;
info->m_maxAngularDof[0] = D_CUSTOM_LARGE_VALUE ;
}
info->m_minAngularDof[1] = 0.0f;
info->m_maxAngularDof[1] = 0.0f;
info->m_minAngularDof[2] = 0.0f;
info->m_maxAngularDof[2] = 0.0f;
memcpy (info->m_attachmenMatrix_0, &m_localMatrix0, sizeof (dMatrix));
memcpy (info->m_attachmenMatrix_1, &m_localMatrix1, sizeof (dMatrix));
}
void SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
dVector p0 (matrix0.m_posit);
dVector p1 (matrix1.m_posit);
dVector dist (p1 - p0);
dFloat mag2 = dist % dist;
if (mag2 > 0.0f) {
dist = dist.Scale (1.0f / dSqrt (mag2));
p1 -= dist.Scale(m_distance);
}
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_right[0]);
}
void CustomSlider::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
// three rows to restrict rotation around around the parent coordinate system
dFloat sinAngle;
dFloat cosAngle;
CalculatePitchAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_front[0]);
CalculateYawAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_up[0]);
CalculateRollAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_right[0]);
// calculate position and speed
dVector veloc0(0.0f, 0.0f, 0.0f, 0.0f);
dVector veloc1(0.0f, 0.0f, 0.0f, 0.0f);
if (m_body0) {
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
}
if (m_body1) {
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
}
m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix1.m_front;
m_speed = (veloc0 - veloc1) % matrix1.m_front;
// if limit are enable ...
m_hitLimitOnLastUpdate = false;
if (m_limitsOn) {
if (m_posit < m_minDist) {
// indicate that this row hit a limit
m_hitLimitOnLastUpdate = true;
// get a point along the up vector and set a constraint
const dVector& p0 = matrix0.m_posit;
dVector p1 (p0 + matrix0.m_front.Scale (m_minDist - m_posit));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
} else if (m_posit > m_maxDist) {
// indicate that this row hit a limit
m_hitLimitOnLastUpdate = true;
// get a point along the up vector and set a constraint
const dVector& p0 = matrix0.m_posit;
dVector p1 (p0 + matrix0.m_front.Scale (m_maxDist - m_posit));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else {
/*
// uncomment this for a slider with friction
// take any point on body0 (origin)
const dVector& p0 = matrix0.m_posit;
dVector veloc0;
dVector veloc1;
dVector omega1;
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
NewtonBodyGetOmega(m_body1, &omega1[0]);
// this assumes the origin of the bodies the matrix pivot are the same
veloc1 += omega1 * (matrix1.m_posit - p0);
dFloat relAccel;
relAccel = ((veloc1 - veloc0) % matrix0.m_front) / timestep;
#define MaxFriction 10.0f
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction (m_joint, -MaxFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, MaxFriction);
*/
}
}
}
void CustomCorkScrew::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
// two rows to restrict rotation around around the parent coordinate system
dFloat sinAngle;
dFloat cosAngle;
CalculateYawAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_up[0]);
CalculateRollAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_right[0]);
// if limit are enable ...
if (m_limitsLinearOn) {
dFloat dist = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
if (dist < m_minLinearDist) {
// get a point along the up vector and set a constraint
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
} else if (dist > m_maxLinearDist) {
// get a point along the up vector and set a constraint
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
}
}
CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle);
dFloat angle = -m_curJointAngle.Update (cosAngle, sinAngle);
if (m_limitsAngularOn) {
// the joint angle can be determine by getting the angle between any two non parallel vectors
if (angle < m_minAngularDist) {
dFloat relAngle = angle - m_minAngularDist;
// the angle was clipped save the new clip limit
//m_curJointAngle.m_angle = m_minAngularDist;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle > m_maxAngularDist) {
dFloat relAngle = angle - m_maxAngularDist;
// the angle was clipped save the new clip limit
//m_curJointAngle.m_angle = m_maxAngularDist;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
}
if (m_angularmotorOn) {
dVector omega0 (0.0f, 0.0f, 0.0f);
dVector omega1 (0.0f, 0.0f, 0.0f);
// get relative angular velocity
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
// calculate the desired acceleration
dFloat relOmega = (omega0 - omega1) % matrix0.m_front;
dFloat relAccel = m_angularAccel - m_angularDamp * relOmega;
// if the motor capability is on, then set angular acceleration with zero angular correction
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
// override the angular acceleration for this Jacobian to the desired acceleration
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
}
void CustomControlledBallAndSocket::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
#if 0
dVector euler0;
dVector euler1;
dMatrix localMatrix (matrix0 * matrix1.Inverse());
localMatrix.GetEulerAngles(euler0, euler1);
AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
euler0 = euler1;
}
dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);
for (int i = 0; i < 3; i ++) {
dFloat error = m_targetAngles[i] - euler[i];
if (dAbs (error) > (0.125f * 3.14159213f / 180.0f) ) {
dFloat angularStep = dSign(error) * m_angulaSpeed * timestep;
if (angularStep > 0.0f) {
if (angularStep > error) {
angularStep = error * 0.5f;
}
} else {
if (angularStep < error) {
angularStep = error * 0.5f;
}
}
euler[i] = euler[i] + angularStep;
}
}
dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
dMatrix baseMatrix (p0y0r0 * matrix1);
dMatrix rotation (matrix0.Inverse() * baseMatrix);
dQuaternion quat (rotation);
if (quat.m_q0 > dFloat (0.99995f)) {
dVector euler0;
dVector euler1;
rotation.GetEulerAngles(euler0, euler1);
NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]);
} else {
dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
NewtonUserJointAddAngularRow (m_joint, 2.0f * dAcos (quat.m_q0), &basis[0][0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[1][0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[2][0]);
}
#else
matrix1 = m_targetRotation * matrix1;
dQuaternion localRotation(matrix1 * matrix0.Inverse());
if (localRotation.DotProduct(m_targetRotation) < 0.0f) {
localRotation.Scale(-1.0f);
}
dFloat angle = 2.0f * dAcos(localRotation.m_q0);
dFloat angleStep = m_angulaSpeed * timestep;
if (angleStep < angle) {
dVector axis(dVector(localRotation.m_q1, localRotation.m_q2, localRotation.m_q3, 0.0f));
axis = axis.Scale(1.0f / dSqrt(axis % axis));
// localRotation = dQuaternion(axis, angleStep);
}
dVector axis (matrix1.m_front * matrix1.m_front);
dVector axis1 (matrix1.m_front * matrix1.m_front);
//dFloat sinAngle;
//dFloat cosAngle;
//CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle);
//float xxxx = dAtan2(sinAngle, cosAngle);
//float xxxx1 = dAtan2(sinAngle, cosAngle);
dQuaternion quat(localRotation);
if (quat.m_q0 > dFloat(0.99995f)) {
// dAssert (0);
/*
dVector euler0;
dVector euler1;
rotation.GetEulerAngles(euler0, euler1);
NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]);
*/
} else {
dMatrix basis(dGrammSchmidt(dVector(quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
NewtonUserJointAddAngularRow(m_joint, -2.0f * dAcos(quat.m_q0), &basis[0][0]);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]);
}
#endif
}
void CustomUniversal::GetInfo (NewtonJointRecord* const info) const
{
strcpy (info->m_descriptionType, GetTypeName());
info->m_attachBody_0 = m_body0;
info->m_attachBody_1 = m_body1;
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
info->m_minLinearDof[0] = 0.0f;
info->m_maxLinearDof[0] = 0.0f;
info->m_minLinearDof[1] = 0.0f;
info->m_maxLinearDof[1] = 0.0f;;
info->m_minLinearDof[2] = 0.0f;
info->m_maxLinearDof[2] = 0.0f;
if (m_limit_0_On) {
dFloat angle;
dFloat sinAngle;
dFloat cosAngle;
sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up;
cosAngle = matrix0.m_front % matrix1.m_front;
angle = dAtan2 (sinAngle, cosAngle);
info->m_minAngularDof[0] = (m_minAngle_0 - angle) * 180.0f / 3.141592f ;
info->m_maxAngularDof[0] = (m_maxAngle_0 - angle) * 180.0f / 3.141592f ;
} else {
info->m_minAngularDof[0] = -D_CUSTOM_LARGE_VALUE ;
info->m_maxAngularDof[0] = D_CUSTOM_LARGE_VALUE ;
}
// info->m_minAngularDof[1] = m_minAngle_1 * 180.0f / 3.141592f;
// info->m_maxAngularDof[1] = m_maxAngle_1 * 180.0f / 3.141592f;
if (m_limit_1_On) {
dFloat angle;
dFloat sinAngle;
dFloat cosAngle;
sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
angle = dAtan2 (sinAngle, cosAngle);
info->m_minAngularDof[1] = (m_minAngle_1 - angle) * 180.0f / 3.141592f ;
info->m_maxAngularDof[1] = (m_maxAngle_1 - angle) * 180.0f / 3.141592f ;
} else {
info->m_minAngularDof[1] = -D_CUSTOM_LARGE_VALUE ;
info->m_maxAngularDof[1] = D_CUSTOM_LARGE_VALUE ;
}
info->m_minAngularDof[2] = 0.0f;
info->m_maxAngularDof[2] = 0.0f;
memcpy (info->m_attachmenMatrix_0, &m_localMatrix0, sizeof (dMatrix));
memcpy (info->m_attachmenMatrix_1, &m_localMatrix1, sizeof (dMatrix));
}
void Custom6DOF::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// add the linear limits
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
dVector dp (p0 - p1);
for (int i = 0; i < 3; i ++) {
if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) {
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
} else {
// it is a limited linear dof, check if it pass the limits
dFloat dist = dp.DotProduct3(matrix1[i]);
if (dist > m_maxLinearLimits[i]) {
dVector q1 (p1 + matrix1[i].Scale (m_maxLinearLimits[i]));
// clamp the error, so the not too much energy is added when constraint violation occurs
dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
if (maxDist > D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
q1 = p0 - matrix1[i].Scale(D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
}
NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (dist < m_minLinearLimits[i]) {
dVector q1 (p1 + matrix1[i].Scale (m_minLinearLimits[i]));
// clamp the error, so the not too much energy is added when constraint violation occurs
dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
if (maxDist < -D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
q1 = p0 - matrix1[i].Scale(-D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
}
NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
}
}
dVector euler0(0.0f);
dVector euler1(0.0f);
dMatrix localMatrix (matrix0 * matrix1.Inverse());
localMatrix.GetEulerAngles(euler0, euler1);
AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
euler0 = euler1;
}
dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);
//dTrace (("(%f %f %f) (%f %f %f)\n", m_pitch.m_angle * 180.0f / 3.141592f, m_yaw.m_angle * 180.0f / 3.141592f, m_roll.m_angle * 180.0f / 3.141592f, euler0.m_x * 180.0f / 3.141592f, euler0.m_y * 180.0f / 3.141592f, euler0.m_z * 180.0f / 3.141592f));
bool limitViolation = false;
for (int i = 0; i < 3; i ++) {
if (euler[i] < m_minAngularLimits[i]) {
limitViolation = true;
euler[i] = m_minAngularLimits[i];
} else if (euler[i] > m_maxAngularLimits[i]) {
limitViolation = true;
euler[i] = m_maxAngularLimits[i];
}
}
if (limitViolation) {
//dMatrix pyr (dPitchMatrix(m_pitch.m_angle) * dYawMatrix(m_yaw.m_angle) * dRollMatrix(m_roll.m_angle));
dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
dMatrix baseMatrix (p0y0r0 * matrix1);
dMatrix rotation (matrix0.Inverse() * baseMatrix);
dQuaternion quat (rotation);
if (quat.m_q0 > dFloat (0.99995f)) {
//dVector p0 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
//dVector p1 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &baseMatrix[2][0]);
//NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
//dVector q0 (matrix0[3] + baseMatrix[0].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[1][0]);
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[2][0]);
} else {
dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
dVector q0 (matrix0[3] + basis[1].Scale (MIN_JOINT_PIN_LENGTH));
dVector q1 (matrix0[3] + rotation.RotateVector(basis[1].Scale (MIN_JOINT_PIN_LENGTH)));
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &basis[2][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
//dVector q0 (matrix0[3] + basis[0].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[1][0]);
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[2][0]);
}
}
}
void CustomLimitBallAndSocket::SubmitConstraints(dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(matrix0, matrix1);
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_right[0]);
matrix1 = m_rotationOffset * matrix1;
// handle special case of the joint being a hinge
if (m_coneAngleCos > 0.9999f) {
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]);
// the joint angle can be determined by getting the angle between any two non parallel vectors
dFloat pitchAngle = CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front);
if ((m_maxTwistAngle - m_minTwistAngle) < 1.0e-4f) {
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix1.m_front[0]);
} else {
if (pitchAngle > m_maxTwistAngle) {
pitchAngle -= m_maxTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -0.0f);
} else if (pitchAngle < m_minTwistAngle) {
pitchAngle -= m_minTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
}
} else {
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngle = coneDir0 % coneDir1;
if (cosAngle <= m_coneAngleCos) {
dVector lateralDir(coneDir0 * coneDir1);
dFloat mag2 = lateralDir % lateralDir;
dAssert(mag2 > 1.0e-4f);
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
dQuaternion rot(m_coneAngleHalfCos, lateralDir.m_x * m_coneAngleHalfSin, lateralDir.m_y * m_coneAngleHalfSin, lateralDir.m_z * m_coneAngleHalfSin);
dVector frontDir(rot.UnrotateVector(coneDir1));
dVector upDir(lateralDir * frontDir);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(coneDir0, frontDir, lateralDir), &lateralDir[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
}
//handle twist angle
dFloat pitchAngle = CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front);
if ((m_maxTwistAngle - m_minTwistAngle) < 1.0e-4f) {
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix1.m_front[0]);
} else {
if (pitchAngle > m_maxTwistAngle) {
pitchAngle -= m_maxTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -0.0f);
} else if (pitchAngle < m_minTwistAngle) {
pitchAngle -= m_minTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
}
}
}
