static void Add6DOF (DemoEntityManager* const scene, const dVector& origin)
{
dVector size (1.0f, 1.0f, 1.0f);
NewtonBody* const box0 = CreateCapule (scene, origin + dVector (0.0f, 5.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateCapule (scene, origin + dVector (0.0f, 5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
const dFloat angle = 60.0f * 3.1415592f / 180.0f;
dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
// connect first box to the world
dMatrix matrix0;
NewtonBodyGetMatrix (box0, & matrix0[0][0]);
pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
Custom6DOF* const joint0 = new Custom6DOF (pinMatrix, pinMatrix, box0, NULL);
joint0->SetAngularLimits (dVector (-angle, -angle, -angle, 0.0f), dVector (angle, angle, angle, 0.0f));
// link the two boxes
dMatrix matrix1;
NewtonBodyGetMatrix (box1, &matrix1[0][0]);
pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
Custom6DOF* const joint1 = new Custom6DOF (pinMatrix, pinMatrix, box0, box1);
joint1->SetAngularLimits (dVector (-angle, -angle, -angle, 0.0f), dVector (angle, angle, angle, 0.0f));
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void AddDistance (DemoEntityManager* const scene, const dVector& origin)
{
dVector size (1.0f, 1.0f, 1.0f);
NewtonBody* const box0 = CreateCapule (scene, origin + dVector (0.0f, 5.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateCapule (scene, origin + dVector (0.0f, 5.0 - size.m_y * 4.0f, 0.0f, 0.0f), size);
dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
// connect first box to the world
dMatrix matrix0;
NewtonBodyGetMatrix (box0, &matrix0[0][0]);
pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
new CustomBallAndSocket (pinMatrix, box0, NULL);
// link the two boxes with a distance joint
dMatrix matrix1;
NewtonBodyGetMatrix (box1, &matrix1[0][0]);
// get the origins
dVector pivot0 (matrix0.m_posit);
dVector pivot1 (matrix1.m_posit);
// connect bodies at a corner
new CustomDistance (pivot1, pivot0, box1, box0);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), NULL, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void AddBallAndSockectWithFriction (DemoEntityManager* const scene, const dVector& origin)
{
dVector size (1.0f, 1.0f, 1.0f);
NewtonBody* const box0 = CreateCapule (scene, origin + dVector (0.0f, 5.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateCapule (scene, origin + dVector (0.0f, 5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
// connect first box to the world
dMatrix matrix0;
NewtonBodyGetMatrix (box0, &matrix0[0][0]);
pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
new CustomBallAndSocketWithFriction (pinMatrix, box0, NULL, 20.0f);
// link the two boxes
dMatrix matrix1;
NewtonBodyGetMatrix (box1, & matrix1[0][0]);
pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
new CustomBallAndSocketWithFriction (pinMatrix, box1, box0, 10.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), NULL, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
CustomRackAndPinion::CustomRackAndPinion(dFloat gearRatio, const dVector& rotationalPin, const dVector& linearPin, NewtonBody* rotationalBody, NewtonBody* linearBody)
:CustomJoint(1, rotationalBody, linearBody)
{
m_gearRatio = gearRatio;
dMatrix dommyMatrix;
dMatrix pinAndPivot0 (dGrammSchmidt (rotationalPin));
CalculateLocalMatrix (pinAndPivot0, m_localMatrix0, dommyMatrix);
m_localMatrix0.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
// calculate the local matrix for body body1
dMatrix pinAndPivot1 (dGrammSchmidt(linearPin));
CalculateLocalMatrix (pinAndPivot1, dommyMatrix, m_localMatrix1);
m_localMatrix1.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
}
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);
}
}
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
CustomPulley::CustomPulley(dFloat gearRatio, const dVector& childPin, const dVector& parentPin, NewtonBody* const child, NewtonBody* const parent)
:CustomJoint(1, child, parent)
{
m_gearRatio = gearRatio;
// calculate the two local matrix of the pivot point
dMatrix dommyMatrix;
// calculate the local matrix for body body0
dMatrix pinAndPivot0 (dGrammSchmidt (childPin));
CalculateLocalMatrix (pinAndPivot0, m_localMatrix0, dommyMatrix);
m_localMatrix0.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
// calculate the local matrix for body body1
dMatrix pinAndPivot1 (dGrammSchmidt (parentPin));
CalculateLocalMatrix (pinAndPivot1, dommyMatrix, m_localMatrix1);
m_localMatrix1.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
}
dCustomUpVector::dCustomUpVector(const dVector& pin, NewtonBody* child)
:dCustomJoint(2, child, NULL)
{
dMatrix pivot;
NewtonBodyGetMatrix(child, &pivot[0][0]);
dMatrix matrix (dGrammSchmidt(pin));
matrix.m_posit = pivot.m_posit;
CalculateLocalMatrix (matrix, m_localMatrix0, m_localMatrix1);
}
static void AddLimitedBallAndSocket (DemoEntityManager* const scene, const dVector& origin)
{
dVector size(1.0f, 1.0f, 1.0f);
NewtonBody* const base = CreateBox(scene, origin + dVector (0.0f, 5.0f + size.m_y + 0.25f, 0.0f, 0.0f), size.Scale (0.2f));
NewtonBody* const box0 = CreateCapule(scene, origin + dVector(0.0f, 5.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateCapule(scene, origin + dVector(0.0f, 5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
NewtonBody* const box2 = CreateCapule(scene, origin + dVector(0.0f, 5.0 - size.m_y * 4.0f, 0.0f, 0.0f), size);
NewtonBodySetMassMatrix(base, 0.0f, 0.0f, 0.0f, 0.0f);
dMatrix pinMatrix(dGrammSchmidt(dVector(0.0f, -1.0f, 0.0f, 0.0f)));
// connect first box to the world
dMatrix matrix;
NewtonBodyGetMatrix(box0, &matrix[0][0]);
pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
CustomLimitBallAndSocket* const joint0 = new CustomLimitBallAndSocket(pinMatrix, box0, base);
joint0->SetConeAngle (30.0f * 3.141592f / 180.0f);
joint0->SetTwistAngle (-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
// connect first box1 to box0 the world
NewtonBodyGetMatrix(box1, &matrix[0][0]);
pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
CustomLimitBallAndSocket* const joint1 = new CustomLimitBallAndSocket(pinMatrix, box1, box0);
joint1->SetConeAngle(30.0f * 3.141592f / 180.0f);
joint1->SetTwistAngle(-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
// connect first box2 to box1 the world
NewtonBodyGetMatrix(box2, &matrix[0][0]);
pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
CustomLimitBallAndSocket* const joint2 = new CustomLimitBallAndSocket(pinMatrix, box2, box1);
joint2->SetConeAngle(30.0f * 3.141592f / 180.0f);
joint2->SetTwistAngle(-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), base, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box0, base);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerAttachBone(skeleton, box2, box1);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
void CustomBallAndSocketWithFriction::SubmitConstraints(dFloat timestep, int threadIndex)
{
CustomBallAndSocket::SubmitConstraints(timestep, threadIndex);
dVector omega0(0.0f, 0.0f, 0.0f, 0.0f);
dVector omega1(0.0f, 0.0f, 0.0f, 0.0f);
// get the omega vector
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
dVector relOmega(omega0 - omega1);
dFloat omegaMag = dSqrt(relOmega % relOmega);
if (omegaMag > 0.1f) {
// tell newton to used this the friction of the omega vector to apply the rolling friction
dMatrix basis(dGrammSchmidt(relOmega));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
// calculate the acceleration to stop the ball in one time step
dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[0][0]);
NewtonUserJointSetRowAcceleration(m_joint, -omegaMag * invTimestep);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
} else {
// when omega is too low this is correct but the small angle approximation theorem.
dMatrix basis(dGetIdentityMatrix());
for (int i = 0; i < 3; i++) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[i][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
}
}
}
// bu animating the orientation of the pin vector the application can change the orientation of the picked object
void dCustomUpVector::SetPinDir (const dVector& pin)
{
m_localMatrix1 = dGrammSchmidt(pin);
}
void CustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex)
{
// check if this is an impulsive time step
if (timestep > 0.0f) {
dMatrix matrix0;
dVector v(0.0f);
dVector w(0.0f);
dVector cg(0.0f);
dFloat invTimestep = 1.0f / timestep;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
NewtonBodyGetOmega (m_body0, &w[0]);
NewtonBodyGetVelocity (m_body0, &v[0]);
NewtonBodyGetCentreOfMass (m_body0, &cg[0]);
NewtonBodyGetMatrix (m_body0, &matrix0[0][0]);
dVector p0 (matrix0.TransformVector (m_localHandle));
dVector pointVeloc (v + w * matrix0.RotateVector (m_localHandle - cg));
dVector relPosit (m_targetPosit - p0);
dVector relVeloc (relPosit.Scale (invTimestep) - pointVeloc);
dVector relAccel (relVeloc.Scale (invTimestep * 0.3f));
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_front);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_up);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_right);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction);
if (m_pickMode) {
dQuaternion rotation;
NewtonBodyGetRotation (m_body0, &rotation.m_q0);
if (m_targetRot.DotProduct (rotation) < 0.0f) {
rotation.m_q0 *= -1.0f;
rotation.m_q1 *= -1.0f;
rotation.m_q2 *= -1.0f;
rotation.m_q3 *= -1.0f;
}
dVector relOmega (rotation.CalcAverageOmega (m_targetRot, invTimestep) - w);
dFloat mag = relOmega % relOmega;
if (mag > 1.0e-6f) {
dVector pin (relOmega.Scale (1.0f / mag));
dMatrix basis (dGrammSchmidt (pin));
dFloat relSpeed = dSqrt (relOmega % relOmega);
dFloat relAlpha = relSpeed * invTimestep;
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
} else {
dVector relAlpha (w.Scale (-invTimestep));
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
}
} else {
// this is the single handle pick mode, add some angular friction
dVector relAlpha = w.Scale (-invTimestep);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f);
}
}
}
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
}
dInfinitePlane (NewtonWorld* const world, const dVector& plane)
:m_minBox (-0.1f, -2000.0f, -2000.0f, 0.0f)
,m_maxBox ( 0.1f, 2000.0f, 2000.0f, 0.0f)
{
// get the transformation matrix that takes the plane to the world local space
m_rotation = dGrammSchmidt(plane);
// build a unit grid in local space (this will be the shadow at projection of the collision aabb)
m_unitSphape[0] = dVector (0.0f, 1.0f, 1.0f);
m_unitSphape[1] = dVector (0.0f, -1.0f, 1.0f);
m_unitSphape[2] = dVector (0.0f, -1.0f, -1.0f);
m_unitSphape[3] = dVector (0.0f, 1.0f, -1.0f);
// save the plane in local space
m_plane = m_rotation.UntransformPlane (plane);
#ifdef PASS_A_QUAD
// passing a single quad
for (int i = 0; i < MAX_THREAD_FACES; i ++) {
m_faceIndices[i][0] = 4;
// face attribute
m_indexArray[i][4] = 0;
// face normal
m_indexArray[i][4 + 1] = 4;
// face area (the plane is clipped around the box, the face size is always optimal)
m_indexArray[i][4 + 2 + 4] = 0;
for (int j = 0; j < 4; j ++) {
// face vertex index
m_indexArray[i][j] = j;
// face adjacent index (infinite plane does not have shared edge with other faces)
m_indexArray[i][j + 4 + 2] = 4;
}
}
#else
// passing two triangle
for (int i = 0; i < MAX_THREAD_FACES; i ++) {
// first triangle
{
// index count
m_faceIndices[i][0] = 3;
// face indices
m_indexArray[i][0] = 0;
m_indexArray[i][1] = 1;
m_indexArray[i][2] = 2;
// face attribute
m_indexArray[i][3] = 0;
// face normal
m_indexArray[i][4] = 4;
// face adjacent index (infinite plane does not have shared edge with other faces)
m_indexArray[i][5] = 4;
m_indexArray[i][6] = 4;
m_indexArray[i][7] = 4;
// face area (the plane is clipped around the box, the face size is always optimal)
m_indexArray[i][8] = 0;
}
// second triangle
{
// index count
m_faceIndices[i][1] = 3;
// face indices
m_indexArray[i][0 + 9] = 0;
m_indexArray[i][1 + 9] = 2;
m_indexArray[i][2 + 9] = 3;
// face attribute
m_indexArray[i][3 + 9] = 0;
// face normal
m_indexArray[i][4 + 9] = 4;
// face adjacent index (infinite plane does not have shared edge with other faces)
m_indexArray[i][5 + 9] = 4;
m_indexArray[i][6 + 9] = 4;
m_indexArray[i][7 + 9] = 4;
// face area (the plane is clipped around the box, the face size is always optimal)
m_indexArray[i][8 + 9] = 0;
}
}
#endif
// create a Newton user collision
m_collision = NewtonCreateUserMeshCollision (world, &m_minBox[0], &m_maxBox[0], this,
PlaneCollisionCollideCallback, PlaneMeshCollisionRayHitCallback,
PlaneCollisionDestroyCallback, PlaneCollisionGetCollisionInfo,
PlaneCollisionAABBOverlapTest, PlaneCollisionGetFacesInAABB,
UserCollisionSerializationCallback, 0);
// set a debug display call back
NewtonStaticCollisionSetDebugCallback (m_collision, ShowMeshCollidingFaces);
// set the collisoin offset Matrix;
NewtonCollisionSetMatrix(m_collision, &m_rotation[0][0]);
}
static void AddPoweredRagDoll (DemoEntityManager* const scene, const dVector& origin)
{
dVector size (1.0f, 1.0f, 1.0f);
NewtonBody* const box0 = CreateCapule(scene, origin + dVector(0.0f, 9.0f, 0.0f, 0.0f), size);
// NewtonBody* const box1 = CreateCapule(scene, origin + dVector(0.0f, 9.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
// NewtonBody* const box2 = CreateCapule(scene, origin + dVector(0.0f, 9.0 - size.m_y * 4.0f, 0.0f, 0.0f), size);
// NewtonBody* const box3 = CreateCapule(scene, origin + dVector(0.0f, 9.0 - size.m_y * 6.0f, 0.0f, 0.0f), size);
dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
//dMatrix pinMatrix (dGrammSchmidt (dVector (1.0f, 0.0f, 0.0f, 0.0f)));
// connect first box to the world
dMatrix matrix0;
NewtonBodyGetMatrix (box0, & matrix0[0][0]);
pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
CustomControlledBallAndSocket* const joint0 = new CustomControlledBallAndSocket (pinMatrix, box0, NULL);
joint0->SetAngularVelocity (2000.0f * 3.141592f / 180.0f);
// joint0->SetPitchAngle (-45.0f * 3.141592f / 180.0f);
// joint0->SetYawAngle (-85.0f * 3.141592f / 180.0f);
// joint0->SetRollAngle (120.0f * 3.141592f / 180.0f);
joint0->SetPitchAngle (90.0f * 3.141592f / 180.0f);
/*
// link the two boxes
dMatrix matrix1;
NewtonBodyGetMatrix (box1, &matrix1[0][0]);
pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
CustomControlledBallAndSocket* const joint1 = new CustomControlledBallAndSocket (pinMatrix, box0, box1);
joint1->SetAngularVelocity (1000.0f * 3.141592f / 180.0f);
joint1->SetPitchAngle (45.0f * 3.141592f / 180.0f);
joint1->SetYawAngle ( 30.0f * 3.141592f / 180.0f);
joint1->SetRollAngle (25.0f * 3.141592f / 180.0f);
// link next box
dMatrix matrix2;
NewtonBodyGetMatrix(box2, &matrix2[0][0]);
pinMatrix.m_posit = (matrix1.m_posit + matrix2.m_posit).Scale(0.5f);
CustomControlledBallAndSocket* const joint2 = new CustomControlledBallAndSocket(pinMatrix, box1, box2);
joint2->SetAngularVelocity(1000.0f * 3.141592f / 180.0f);
joint2->SetPitchAngle(45.0f * 3.141592f / 180.0f);
joint2->SetYawAngle(30.0f * 3.141592f / 180.0f);
joint2->SetRollAngle(25.0f * 3.141592f / 180.0f);
// link next box
dMatrix matrix3;
NewtonBodyGetMatrix(box3, &matrix3[0][0]);
pinMatrix.m_posit = (matrix2.m_posit + matrix3.m_posit).Scale(0.5f);
CustomControlledBallAndSocket* const joint3 = new CustomControlledBallAndSocket(pinMatrix, box2, box3);
joint3->SetAngularVelocity(1000.0f * 3.141592f / 180.0f);
joint3->SetPitchAngle(45.0f * 3.141592f / 180.0f);
joint3->SetYawAngle(30.0f * 3.141592f / 180.0f);
joint3->SetRollAngle(25.0f * 3.141592f / 180.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerAttachBone(skeleton, box2, box1);
NewtonSkeletonContainerAttachBone(skeleton, box3, box2);
NewtonSkeletonContainerFinalize(skeleton);
#endif
*/
}
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 dCustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex)
{
// check if this is an impulsive time step
dMatrix matrix0(GetBodyMatrix());
dVector omega(0.0f);
dVector com(0.0f);
dVector pointVeloc(0.0f);
const dFloat damp = 0.3f;
dAssert (timestep > 0.0f);
const dFloat invTimestep = 1.0f / timestep;
// we not longer cap excessive angular velocities, it is left to the client application.
NewtonBodyGetOmega(m_body0, &omega[0]);
//cap excessive angular velocities
dFloat mag2 = omega.DotProduct3(omega);
if (mag2 > (m_omegaCap * m_omegaCap)) {
omega = omega.Normalize().Scale(m_omegaCap);
NewtonBodySetOmega(m_body0, &omega[0]);
}
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
dVector relPosit(m_targetMatrix.m_posit - matrix0.m_posit);
NewtonBodyGetPointVelocity(m_body0, &m_targetMatrix.m_posit[0], &pointVeloc[0]);
for (int i = 0; i < 3; i ++) {
// Restrict the movement on the pivot point along all tree orthonormal direction
dFloat speed = pointVeloc.DotProduct3(m_targetMatrix[i]);
dFloat dist = relPosit.DotProduct3(m_targetMatrix[i]) * damp;
dFloat relSpeed = dist * invTimestep - speed;
dFloat relAccel = relSpeed * invTimestep;
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &m_targetMatrix[i][0]);
NewtonUserJointSetRowAcceleration(m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_maxLinearFriction);
}
if (m_isSixdof) {
dQuaternion rotation (matrix0.Inverse() * m_targetMatrix);
if (dAbs (rotation.m_q0) < 0.99998f) {
dMatrix rot (dGrammSchmidt(dVector (rotation.m_q1, rotation.m_q2, rotation.m_q3)));
dFloat angle = 2.0f * dAcos(dClamp(rotation.m_q0, dFloat(-1.0f), dFloat(1.0f)));
NewtonUserJointAddAngularRow (m_joint, angle, &rot.m_front[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &rot.m_up[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &rot.m_right[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
} else {
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
}
}
}
