void dCustomCorkScrew::SubmitConstraintLimitSpringDamper(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
dFloat angle = m_curJointAngle.GetAngle() + m_angularOmega * timestep;
if (angle < m_minAngle) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
const dFloat invtimestep = 1.0f / timestep;
const dFloat speed = 0.5f * (m_minAngle - m_curJointAngle.GetAngle()) * invtimestep;
const dFloat springAccel = NewtonCalculateSpringDamperAcceleration(timestep, m_angularSpring, m_curJointAngle.GetAngle(), m_angularDamper, m_angularOmega);
const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep + springAccel;
NewtonUserJointSetRowAcceleration(m_joint, stopAccel);
} else if (angle > m_maxAngle) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
const dFloat invtimestep = 1.0f / timestep;
const dFloat speed = 0.5f * (m_maxAngle - m_curJointAngle.GetAngle()) * invtimestep;
const dFloat springAccel = NewtonCalculateSpringDamperAcceleration(timestep, m_angularSpring, m_curJointAngle.GetAngle(), m_angularDamper, m_angularOmega);
const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep + springAccel;
NewtonUserJointSetRowAcceleration(m_joint, stopAccel);
} else {
dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep);
}
}
void dCustomBallAndSocket::SubmitConstraintTwistLimits(const dMatrix& matrix0, const dMatrix& matrix1, const dVector& relOmega, dFloat timestep)
{
dFloat jointOmega = relOmega.DotProduct3(matrix0.m_front);
dFloat twistAngle = m_twistAngle.GetAngle() + jointOmega * timestep;
if (twistAngle < m_minTwistAngle) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);
const dFloat invtimestep = 1.0f / timestep;
const dFloat speed = 0.5f * (m_minTwistAngle - m_twistAngle.GetAngle()) * invtimestep;
const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
NewtonUserJointSetRowAcceleration(m_joint, stopAccel);
} else if (twistAngle > m_maxTwistAngle) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);
const dFloat invtimestep = 1.0f / timestep;
const dFloat speed = 0.5f * (m_maxTwistAngle - m_twistAngle.GetAngle()) * invtimestep;
const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
NewtonUserJointSetRowAcceleration(m_joint, stopAccel);
} else if (m_twistFriction > 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
dFloat accel = NewtonUserJointCalculateRowZeroAccelaration(m_joint);
NewtonUserJointSetRowAcceleration(m_joint, accel);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);
}
}
void MSP::PointToPoint::submit_constraints(const NewtonJoint* joint, dFloat timestep, int thread_index) {
MSP::Joint::JointData* joint_data = reinterpret_cast<MSP::Joint::JointData*>(NewtonJointGetUserData(joint));
PointToPointData* cj_data = reinterpret_cast<PointToPointData*>(joint_data->m_cj_data);
dFloat inv_timestep = 1.0f / timestep;
dMatrix matrix0, matrix1;
MSP::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1);
dVector p0(matrix0.m_posit + matrix0.m_right.Scale(cj_data->m_start_distance));
const dVector& p1 = matrix1.m_posit;
dVector veloc0(0.0f);
dVector veloc1(0.0f);
NewtonBodyGetVelocity(joint_data->m_child, &veloc0[0]);
if (joint_data->m_parent != nullptr)
NewtonBodyGetVelocity(joint_data->m_parent, &veloc1[0]);
dVector rel_veloc(veloc0 - veloc1);
dFloat stiffness = 0.999f - (1.0f - joint_data->m_stiffness_ratio * cj_data->m_strength) * Joint::DEFAULT_STIFFNESS_RANGE;
if (cj_data->m_mode == 0) {
dVector dir(p0 - p1);
cj_data->m_cur_distance = Util::get_vector_magnitude(dir);
if (cj_data->m_cur_distance > M_EPSILON)
Util::scale_vector(dir, 1.0f / cj_data->m_cur_distance);
else {
dir.m_x = 0.0f;
dir.m_y = 0.0f;
dir.m_z = 1.0f;
}
dFloat offset = cj_data->m_cur_distance - cj_data->m_start_distance * cj_data->m_controller;
dFloat dir_veloc = rel_veloc.DotProduct3(dir);
dFloat des_accel = cj_data->m_accel * -offset - dir_veloc * inv_timestep * cj_data->m_damp;
NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &dir[0]);
NewtonUserJointSetRowAcceleration(joint, des_accel);
NewtonUserJointSetRowStiffness(joint, stiffness);
}
else {
dVector offset(matrix1.UntransformVector(p0));
cj_data->m_cur_distance = Util::get_vector_magnitude(offset);
offset.m_z -= cj_data->m_start_distance * cj_data->m_controller;
dVector loc_vel(matrix1.UnrotateVector(rel_veloc));
dVector des_accel(offset.Scale(-cj_data->m_accel) - loc_vel.Scale(inv_timestep * cj_data->m_damp));
NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix1.m_front[0]);
NewtonUserJointSetRowAcceleration(joint, des_accel.m_x);
NewtonUserJointSetRowStiffness(joint, stiffness);
NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix1.m_up[0]);
NewtonUserJointSetRowAcceleration(joint, des_accel.m_y);
NewtonUserJointSetRowStiffness(joint, stiffness);
NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix1.m_right[0]);
NewtonUserJointSetRowAcceleration(joint, des_accel.m_z);
NewtonUserJointSetRowStiffness(joint, stiffness);
}
}
// rolling friction works as follow: the idealization of the contact of a spherical object
// with a another surface is a point that pass by the center of the sphere.
// in most cases this is enough to model the collision but in insufficient for modeling
// the rolling friction. In reality contact with the sphere with the other surface is not
// a point but a contact patch. A contact patch has the property the it generates a fix
// constant rolling torque that opposes the movement of the sphere.
// we can model this torque by adding a clamped torque aligned to the instantaneously axis
// of rotation of the ball. and with a magnitude of the stopping angular acceleration.
void CustomDryRollingFriction::SubmitConstrainst (dFloat timestep, int threadIndex)
{
dVector omega;
dFloat omegaMag;
dFloat torqueFriction;
// get the omega vector
NewtonBodyGetOmega(m_body0, &omega[0]);
omegaMag = dSqrt (omega % omega);
if (omegaMag > 0.1f) {
// tell newton to used this the friction of the omega vector to apply the rolling friction
dVector pin (omega.Scale (1.0f / omegaMag));
NewtonUserJointAddAngularRow (m_joint, 0.0f, &pin[0]);
// calculate the acceleration to stop the ball in one time step
NewtonUserJointSetRowAcceleration (m_joint, -omegaMag / timestep);
// set the friction limit proportional the sphere Inertia
torqueFriction = m_frictionTorque * m_frictionCoef;
NewtonUserJointSetRowMinimumFriction (m_joint, -torqueFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, torqueFriction);
} else {
// when omega is too low sheath a little bit and damp the omega directly
omega = omega.Scale (0.2f);
NewtonBodySetOmega(m_body0, &omega[0]);
}
}
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 dCustomHinge::SubmitConstraintsFrictionAndLimit(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
dFloat angle = GetPitch();
if (angle < m_minAngle) {
dFloat relAngle = m_minAngle - angle;
NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_friction);
// m_lastRowWasUsed = true;
} else if (angle > m_maxAngle) {
dFloat relAngle = m_maxAngle - angle;
NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowMaximumFriction(m_joint, m_friction);
// m_lastRowWasUsed = true;
} else {
// friction but not limits
dFloat alpha = m_jointOmega / timestep;
NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
NewtonUserJointSetRowAcceleration(m_joint, -alpha);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_friction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_friction);
// m_lastRowWasUsed = true;
}
}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
const dFloat angleError = GetMaxAngleError();
dFloat angle0 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up);
NewtonUserJointAddAngularRow(m_joint, angle0, &matrix1.m_up[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
if (dAbs(angle0) > angleError) {
const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle0 / (timestep * timestep);
NewtonUserJointSetRowAcceleration(m_joint, alpha);
}
dFloat angle1 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right);
NewtonUserJointAddAngularRow(m_joint, angle1, &matrix1.m_right[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
if (dAbs(angle1) > angleError) {
const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle1 / (timestep * timestep);
NewtonUserJointSetRowAcceleration(m_joint, alpha);
}
// the joint angle can be determined by getting the angle between any two non parallel vectors
m_curJointAngle.Update(-CalculateAngle(matrix0.m_up, matrix1.m_up, matrix1.m_front));
// 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_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);
if (m_options.m_option2) {
if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep);
} else {
dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep);
}
} else if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep);
} else if (m_angularFriction != 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
}
}
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 dCustomHinge::SubmitConstraintsFrictionOnly(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
dFloat alpha = m_jointOmega / timestep;
NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
NewtonUserJointSetRowAcceleration(m_joint, -alpha);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_friction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_friction);
}
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 dAnimationRigHinge::SubmitConstraints (dFloat timestep, int threadIndex)
{
dCustomHinge::SubmitConstraints (timestep, threadIndex);
// dFloat angle = GetJointAngle();
// dFloat speed = GetJointOmega();
if (!m_limitReached) {
NewtonJoint* const joint = dCustomHinge::GetJoint();
NewtonUserJointSetRowAcceleration(joint, m_rowAccel);
}
}
void MSNewton::Fixed::submit_constraints(const NewtonJoint* joint, dgFloat32 timestep, int thread_index) {
JointData* joint_data = (JointData*)NewtonJointGetUserData(joint);
// Calculate position of pivot points and Jacobian direction vectors in global space.
dMatrix matrix0, matrix1, matrix2;
MSNewton::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1, matrix2);
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
// Get a point along the pin axis at some reasonable large distance from the pivot.
dVector q0(p0 + matrix0.m_right.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1(p1 + matrix1.m_right.Scale(MIN_JOINT_PIN_LENGTH));
// Get the ankle point.
dVector r0(p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector r1(p1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// Restrict movement on the pivot point along all three orthonormal directions
NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix0.m_front[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix0.m_up[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix0.m_right[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
// Restrict rotation along all three orthonormal directions
NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &matrix0.m_front[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &matrix0.m_up[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddLinearRow(joint, &r0[0], &r1[0], &matrix0.m_up[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
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 dCustomHingeActuator::SubmitConstraintsFreeDof (dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1)
{
if (m_actuatorFlag) {
dFloat jointangle = GetActuatorAngle();
dFloat relAngle = jointangle - m_angle;
dFloat currentSpeed = GetJointOmega();
dFloat step = dFloat(2.0f) * m_angularRate * timestep;
dFloat desiredSpeed = (dAbs(relAngle) > dAbs(step)) ? -dSign(relAngle) * m_angularRate : -dFloat(0.1f) * relAngle / timestep;
dFloat accel = (desiredSpeed - currentSpeed) / timestep;
NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration(m_joint, accel);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxForce);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
} else {
dCustomHinge::SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
}
}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
dMatrix localMatrix(matrix0 * matrix1.Inverse());
dVector euler0;
dVector euler1;
localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);
dVector rollPin(dSin(euler0[1]), dFloat(0.0f), dCos(euler0[1]), dFloat(0.0f));
rollPin = matrix1.RotateVector(rollPin);
NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, -euler0[2], &rollPin[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
// the joint angle can be determined by getting the angle between any two non parallel vectors
m_curJointAngle.Update(euler0.m_x);
// 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_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);
if (m_options.m_option2) {
if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep);
} else {
dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep);
}
} else if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep);
} else if (m_angularFriction != 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
}
}
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);
}
}
}
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 MSP::BallAndSocket::submit_constraints(const NewtonJoint* joint, dFloat timestep, int thread_index) {
MSP::Joint::JointData* joint_data = reinterpret_cast<MSP::Joint::JointData*>(NewtonJointGetUserData(joint));
BallAndSocketData* cj_data = reinterpret_cast<BallAndSocketData*>(joint_data->m_cj_data);
dFloat inv_timestep = 1.0f / timestep;
// Calculate the position of the pivot point and the Jacobian direction vectors, in global space.
dMatrix matrix0, matrix1;
MSP::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1);
dFloat last_cone_angle = cj_data->m_cur_cone_angle;
// Calculate current cone angle
dFloat cur_cone_angle_cos = matrix0.m_right.DotProduct3(matrix1.m_right);
cj_data->m_cur_cone_angle = dAcos(Util::clamp_float(cur_cone_angle_cos, -1.0f, 1.0f));
// Calculate current twist angle, omega, and acceleration.
if (cur_cone_angle_cos < -0.999999f) {
cj_data->m_cur_twist_omega = 0.0f;
cj_data->m_cur_twist_alpha = 0.0f;
}
else {
dFloat last_twist_angle = cj_data->m_twist_ai->get_angle();
dFloat last_twist_omega = cj_data->m_cur_twist_omega;
dMatrix rot_matrix0;
Util::rotate_matrix_to_dir(matrix0, matrix1.m_right, rot_matrix0);
dFloat sin_angle;
dFloat cos_angle;
MSP::Joint::c_calculate_angle(matrix1.m_front, rot_matrix0.m_front, matrix1.m_right, sin_angle, cos_angle);
cj_data->m_twist_ai->update(cos_angle, sin_angle);
cj_data->m_cur_twist_omega = (cj_data->m_twist_ai->get_angle() - last_twist_angle) * inv_timestep;
cj_data->m_cur_twist_alpha = (cj_data->m_cur_twist_omega - last_twist_omega) * inv_timestep;
}
// Get the current lateral and tangent dir
dVector lateral_dir;
dVector front_dir;
if (dAbs(cur_cone_angle_cos) > 0.999999f) {
lateral_dir = matrix1.m_front;
front_dir = matrix1.m_up;
}
else {
lateral_dir = matrix1.m_right.CrossProduct(matrix0.m_right);
front_dir = matrix1.m_right.CrossProduct(lateral_dir);
}
// Restrict the movement on the pivot point along all tree orthonormal directions.
NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
// Calculate friction
dFloat power = cj_data->m_friction * dAbs(cj_data->m_controller);
// Handle cone angle
if (cj_data->m_cone_limits_enabled && cj_data->m_max_cone_angle < Joint::ANGULAR_LIMIT_EPSILON2) {
// Handle in case joint being a hinge; max cone angle is near zero.
NewtonUserJointAddAngularRow(joint, MSP::Joint::c_calculate_angle2(matrix0.m_right, matrix1.m_right, matrix1.m_front), &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
NewtonUserJointAddAngularRow(joint, MSP::Joint::c_calculate_angle2(matrix0.m_right, matrix1.m_right, matrix1.m_up), &matrix1.m_up[0]);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
}
else if (cj_data->m_cone_limits_enabled && cj_data->m_cur_cone_angle > cj_data->m_max_cone_angle) {
// Handle in case current cone angle is greater than max cone angle
dFloat dangle = cj_data->m_cur_cone_angle - cj_data->m_max_cone_angle;
NewtonUserJointAddAngularRow(joint, dangle, &lateral_dir[0]);
NewtonUserJointSetRowMaximumFriction(joint, 0.0f);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
NewtonUserJointAddAngularRow(joint, 0.0f, &front_dir[0]);
NewtonUserJointSetRowMinimumFriction(joint, -power);
NewtonUserJointSetRowMaximumFriction(joint, power);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
}
else {
// Handle in case limits are not necessary
dFloat cur_cone_omega = (cj_data->m_cur_cone_angle - last_cone_angle) * inv_timestep;
dFloat des_cone_accel = -cur_cone_omega * inv_timestep;
NewtonUserJointAddAngularRow(joint, 0.0f, &lateral_dir[0]);
NewtonUserJointSetRowAcceleration(joint, des_cone_accel);
NewtonUserJointSetRowMinimumFriction(joint, -power);
NewtonUserJointSetRowMaximumFriction(joint, power);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
NewtonUserJointAddAngularRow(joint, 0.0f, &front_dir[0]);
NewtonUserJointSetRowMinimumFriction(joint, -power);
NewtonUserJointSetRowMaximumFriction(joint, power);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
}
// Handle twist angle
bool bcontinue = false;
if (cj_data->m_twist_limits_enabled) {
if (cj_data->m_min_twist_angle > cj_data->m_max_twist_angle) {
// Handle in case min angle is greater than max
NewtonUserJointAddAngularRow(joint, (cj_data->m_min_twist_angle + cj_data->m_max_twist_angle) * 0.5f - cj_data->m_twist_ai->get_angle(), &matrix0.m_right[0]);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
}
else if (cj_data->m_max_twist_angle - cj_data->m_min_twist_angle < Joint::ANGULAR_LIMIT_EPSILON2) {
// Handle in case min angle is almost equal to max
NewtonUserJointAddAngularRow(joint, cj_data->m_max_twist_angle - cj_data->m_twist_ai->get_angle(), &matrix0.m_right[0]);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
}
else if (cj_data->m_twist_ai->get_angle() < cj_data->m_min_twist_angle) {
// Handle in case current twist angle is less than min
NewtonUserJointAddAngularRow(joint, cj_data->m_min_twist_angle - cj_data->m_twist_ai->get_angle() + Joint::ANGULAR_LIMIT_EPSILON, &matrix0.m_right[0]);
NewtonUserJointSetRowMinimumFriction(joint, 0.0f);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
}
else if (cj_data->m_twist_ai->get_angle() > cj_data->m_max_twist_angle) {
// Handle in case current twist angle is greater than max
NewtonUserJointAddAngularRow(joint, cj_data->m_max_twist_angle - cj_data->m_twist_ai->get_angle() - Joint::ANGULAR_LIMIT_EPSILON, &matrix0.m_right[0]);
NewtonUserJointSetRowMaximumFriction(joint, 0.0f);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
}
else
bcontinue = true;
}
else
bcontinue = true;
if (bcontinue) {
// Handle in case limits are not necessary
NewtonUserJointAddAngularRow(joint, 0.0f, &matrix0.m_right[0]);
NewtonUserJointSetRowAcceleration(joint, -cj_data->m_cur_twist_omega * inv_timestep);
NewtonUserJointSetRowMinimumFriction(joint, -power);
NewtonUserJointSetRowMaximumFriction(joint, power);
NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
}
}
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);
}
}
}
void MSNewton::Servo::submit_constraints(const NewtonJoint* joint, dgFloat32 timestep, int thread_index) {
JointData* joint_data = (JointData*)NewtonJointGetUserData(joint);
ServoData* cj_data = (ServoData*)joint_data->cj_data;
// Calculate position of pivot points and Jacobian direction vectors in global space.
dMatrix matrix0, matrix1, matrix2;
MSNewton::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1, matrix2);
// Calculate angle, omega, and acceleration.
dFloat last_angle = cj_data->ai->get_angle();
dFloat last_omega = cj_data->cur_omega;
dFloat sin_angle;
dFloat cos_angle;
Joint::c_calculate_angle(matrix1.m_front, matrix0.m_front, matrix0.m_right, sin_angle, cos_angle);
cj_data->ai->update(cos_angle, sin_angle);
cj_data->cur_omega = (cj_data->ai->get_angle() - last_angle) / timestep;
cj_data->cur_accel = (cj_data->cur_omega - last_omega) / timestep;
dFloat cur_angle = cj_data->ai->get_angle();
// Restrict movement on the pivot point along all tree orthonormal directions.
NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_front[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
// Add two rows to restrict rotation around the the axis perpendicular to the rotation axis.
/*NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_right, matrix1.m_right, matrix0.m_front), &matrix0.m_front[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_right, matrix1.m_right, matrix0.m_up), &matrix0.m_up[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);*/
// Add two more rows to achieve a more robust angular constraint.
// Get a point along the pin axis at some reasonable large distance from the pivot.
dVector q0(matrix0.m_posit + matrix0.m_right.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1(matrix1.m_posit + matrix1.m_right.Scale(MIN_JOINT_PIN_LENGTH));
// Add two constraints row perpendicular to the pin vector.
NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &matrix1.m_front[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &matrix1.m_up[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
// Add limits and friction
if (cj_data->limits_enabled == true && cur_angle < cj_data->min - Joint::ANGULAR_LIMIT_EPSILON) {
dFloat rel_angle = cj_data->min - cur_angle;
NewtonUserJointAddAngularRow(joint, rel_angle, &matrix0.m_right[0]);
NewtonUserJointSetRowMinimumFriction(joint, 0.0f);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
else if (cj_data->limits_enabled == true && cur_angle > cj_data->max + Joint::ANGULAR_LIMIT_EPSILON) {
dFloat rel_angle = cj_data->max - cur_angle;
NewtonUserJointAddAngularRow(joint, rel_angle, &matrix0.m_right[0]);
NewtonUserJointSetRowMaximumFriction(joint, 0.0f);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
else {
if (cj_data->controller_enabled) {
// Get relative angular velocity
dVector omega0(0.0f, 0.0f, 0.0f);
dVector omega1(0.0f, 0.0f, 0.0f);
NewtonBodyGetOmega(joint_data->child, &omega0[0]);
if (joint_data->parent != nullptr)
NewtonBodyGetOmega(joint_data->parent, &omega1[0]);
dFloat rel_omega = (omega0 - omega1) % matrix1.m_right;
// Calculate relative angle
dFloat desired_angle = cj_data->limits_enabled ? Util::clamp(cj_data->controller, cj_data->min, cj_data->max) : cj_data->controller;
dFloat rel_angle = desired_angle - cur_angle;
dFloat arel_angle = dAbs(rel_angle);
// Calculate desired accel
dFloat mar = cj_data->rate * cj_data->reduction_ratio;
dFloat ratio = (cj_data->rate > EPSILON && cj_data->reduction_ratio > EPSILON && arel_angle < mar) ? arel_angle / mar : 1.0f;
dFloat step = cj_data->rate * ratio * dSign(rel_angle) * timestep;
if (dAbs(step) > arel_angle) step = rel_angle;
dFloat desired_omega = step / timestep;
dFloat desired_accel = (desired_omega - rel_omega) / timestep;
// Add angular row
NewtonUserJointAddAngularRow(joint, step, &matrix0.m_right[0]);
// Apply acceleration
NewtonUserJointSetRowAcceleration(joint, desired_accel);
}
else {
// Add angular row
NewtonUserJointAddAngularRow(joint, 0.0f, &matrix1.m_right[0]);
}
if (cj_data->power == 0.0f) {
NewtonUserJointSetRowMinimumFriction(joint, -Joint::CUSTOM_LARGE_VALUE);
NewtonUserJointSetRowMaximumFriction(joint, Joint::CUSTOM_LARGE_VALUE);
}
else {
NewtonUserJointSetRowMinimumFriction(joint, -cj_data->power);
NewtonUserJointSetRowMaximumFriction(joint, cj_data->power);
}
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
}
void CustomCorkScrew::SubmitConstrainst ()
{
dFloat dist;
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 movement on the pivot point along all tree orthonormal direction
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], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_right[0]);
// get a point along the pin axis at some reasonable large distance from the pivot
dVector q0 (p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1 (p1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// two constraints row perpendiculars to the hinge pin
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) {
dist = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
if (dist < m_minDist) {
// 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_maxDist) {
// 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);
}
}
if (m_angularmotorOn) {
dFloat relOmega;
dFloat relAccel;
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
relOmega = (omega0 - omega1) % matrix0.m_front;
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 MSNewton::Slider::submit_constraints(const NewtonJoint* joint, dgFloat32 timestep, int thread_index) {
JointData* joint_data = (JointData*)NewtonJointGetUserData(joint);
SliderData* cj_data = (SliderData*)joint_data->cj_data;
// Calculate position of pivot points and Jacobian direction vectors in global space.
dMatrix matrix0, matrix1, matrix2;
MSNewton::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1, matrix2);
const dVector& pos0 = matrix0.m_posit;
dVector pos1(matrix1.m_posit + matrix1.m_right.Scale((pos0 - matrix1.m_posit) % matrix1.m_right));
// Calculate position, velocity, and acceleration
dFloat last_pos = cj_data->cur_pos;
dFloat last_vel = cj_data->cur_vel;
cj_data->cur_pos = matrix1.UntransformVector(matrix0.m_posit).m_z;
cj_data->cur_vel = (cj_data->cur_pos - last_pos) / timestep;
cj_data->cur_accel = (cj_data->cur_vel - last_vel) / timestep;
// Restrict movement on axis perpendicular to the pin direction.
NewtonUserJointAddLinearRow(joint, &pos0[0], &pos1[0], &matrix0.m_front[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddLinearRow(joint, &pos0[0], &pos1[0], &matrix0.m_up[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
// Add three angular rows to restrict rotation around all axis.
/*NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_right, matrix1.m_right, matrix0.m_front), &matrix0.m_front[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_right, matrix1.m_right, matrix0.m_up), &matrix0.m_up[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_front, matrix1.m_front, matrix0.m_right), &matrix0.m_right[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);*/
// Get a point along the ping axis at some reasonable large distance from the pivot
dVector q0(pos0 + matrix0.m_right.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1(pos1 + matrix1.m_right.Scale(MIN_JOINT_PIN_LENGTH));
// Add two constraints row perpendicular to the pin
NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &matrix0.m_front[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &matrix0.m_up[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
// Get a point along the ping axis at some reasonable large distance from the pivot
dVector r0(pos0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector r1(pos1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// Add one constraint row perpendicular to the pin
NewtonUserJointAddLinearRow(joint, &r0[0], &r1[0], &matrix0.m_up[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
// Add limits and friction
if (cj_data->limits_enabled == true && cj_data->cur_pos < cj_data->min - Joint::LINEAR_LIMIT_EPSILON) {
const dVector& s0 = matrix0.m_posit;
dVector s1(s0 + matrix1.m_right.Scale(cj_data->min - cj_data->cur_pos));
NewtonUserJointAddLinearRow(joint, &s0[0], &s1[0], &matrix1.m_right[0]);
NewtonUserJointSetRowMinimumFriction(joint, 0.0f);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
else if (cj_data->limits_enabled == true && cj_data->cur_pos > cj_data->max + Joint::LINEAR_LIMIT_EPSILON) {
const dVector& s0 = matrix0.m_posit;
dVector s1(s0 + matrix1.m_right.Scale(cj_data->max - cj_data->cur_pos));
NewtonUserJointAddLinearRow(joint, &s0[0], &s1[0], &matrix1.m_right[0]);
NewtonUserJointSetRowMaximumFriction(joint, 0.0f);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
else {
dVector point(matrix1.UntransformVector(matrix0.m_posit));
point.m_z = 0.0f;
point = matrix1.TransformVector(point);
NewtonUserJointAddLinearRow(joint, &point[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
dFloat power = cj_data->friction * cj_data->controller;
/*BodyData* cbody_data = (BodyData*)NewtonBodyGetUserData(joint_data->child);
if (cbody_data->bstatic == false && cbody_data->mass >= MIN_MASS)
power *= cbody_data->mass;
else {
BodyData* pbody_data = (BodyData*)NewtonBodyGetUserData(joint_data->child);
if (pbody_data->bstatic == false && pbody_data->mass >= MIN_MASS) power *= pbody_data->mass;
}*/
NewtonUserJointSetRowMinimumFriction(joint, -power);
NewtonUserJointSetRowMaximumFriction(joint, power);
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
}
void CustomHinge::SubmitConstraintsFreeDof(dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1)
{
// four possibilities
dFloat angle = m_curJointAngle.GetAngle();
if (m_friction != 0.0f) {
if (m_limitsOn) {
// friction and limits at the same time
if (angle < m_minAngle) {
dFloat relAngle = angle - m_minAngle;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow(m_joint, -relAngle, &matrix1.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness(m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
m_lastRowWasUsed = true;
} else if (angle > m_maxAngle) {
dFloat relAngle = angle - m_maxAngle;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow(m_joint, -relAngle, &matrix1.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness(m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
m_lastRowWasUsed = true;
} else {
// friction but not limits
dFloat alpha = m_jointOmega / timestep;
NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
NewtonUserJointSetRowAcceleration(m_joint, -alpha);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_friction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_friction);
NewtonUserJointSetRowStiffness(m_joint, 1.0f);
m_lastRowWasUsed = true;
}
} else {
// friction but not limits
dFloat alpha = m_jointOmega / timestep;
NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
NewtonUserJointSetRowAcceleration(m_joint, -alpha);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_friction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_friction);
NewtonUserJointSetRowStiffness(m_joint, 1.0f);
m_lastRowWasUsed = true;
}
} else if (m_limitsOn) {
// only limit are on
// the joint angle can be determine by getting the angle between any two non parallel vectors
if ((m_minAngle > -1.e-4f) && (m_maxAngle < 1.e-4f)) {
NewtonUserJointAddAngularRow(m_joint, -angle, &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, 1.0f);
m_lastRowWasUsed = true;
} else if (angle < m_minAngle) {
dFloat relAngle = angle - m_minAngle;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow(m_joint, -relAngle, &matrix1.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness(m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
m_lastRowWasUsed = true;
} else if (angle > m_maxAngle) {
dFloat relAngle = angle - m_maxAngle;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow(m_joint, -relAngle, &matrix1.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness(m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
m_lastRowWasUsed = true;
}
}
}
void CustomUniversal::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]);
// construct an orthogonal coordinate system with these two vectors
dMatrix matrix1_1;
matrix1_1.m_up = matrix1.m_up;
matrix1_1.m_right = matrix0.m_front * matrix1.m_up;
matrix1_1.m_right = matrix1_1.m_right.Scale (1.0f / dSqrt (matrix1_1.m_right % matrix1_1.m_right));
matrix1_1.m_front = matrix1_1.m_up * matrix1_1.m_right;
NewtonUserJointAddAngularRow (m_joint, CalculateAngle (matrix0.m_front, matrix1_1.m_front, matrix1_1.m_right), &matrix1_1.m_right[0]);
dFloat sinAngle_0;
dFloat cosAngle_0;
CalculateAngle (matrix1_1.m_up, matrix0.m_up, matrix1_1.m_front, sinAngle_0, cosAngle_0);
dFloat angle0 = -m_curJointAngle_0.Update (cosAngle_0, sinAngle_0);
dFloat sinAngle_1;
dFloat cosAngle_1;
CalculateAngle(matrix1.m_front, matrix1_1.m_front, matrix1_1.m_up, sinAngle_1, cosAngle_1);
dFloat angle1 = -m_curJointAngle_1.Update (cosAngle_1, sinAngle_1);
dVector omega0 (0.0f, 0.0f, 0.0f, 0.0f);
dVector omega1 (0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
// calculate the desired acceleration
dVector relOmega (omega0 - omega1);
m_jointOmega_0 = relOmega % matrix0.m_front;
m_jointOmega_1 = relOmega % matrix1.m_up;
// check is the joint limit are enable
if (m_limit_0_On) {
if (angle0 < m_minAngle_0) {
dFloat relAngle = angle0 - m_minAngle_0;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle0 > m_maxAngle_0) {
dFloat relAngle = angle0 - m_maxAngle_0;
// 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);
}
// check is the joint limit motor is enable
} else if (m_angularMotor_0_On) {
// calculate the desired acceleration
// dFloat relOmega = (omega0 - omega1) % matrix0.m_front;
dFloat relAccel = m_angularAccel_0 - m_angularDamp_0 * m_jointOmega_0;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
// if limit are enable ...
if (m_limit_1_On) {
if (angle1 < m_minAngle_1) {
dFloat relAngle = angle1 - m_minAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle1 > m_maxAngle_1) {
dFloat relAngle = angle1 - m_maxAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
} else if (m_angularMotor_1_On) {
// calculate the desired acceleration
dFloat relAccel = m_angularAccel_1 - m_angularDamp_1 * m_jointOmega_1;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix1.m_up[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
}
void CustomGear::SubmitConstrainst (dFloat timestep, int threadIndex)
{
dFloat w0;
dFloat w1;
dFloat relAccel;
dFloat relOmega;
dVector omega0;
dVector omega1;
dMatrix matrix0;
dMatrix matrix1;
dFloat jacobian0[6];
dFloat jacobian1[6];
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
// calculate the angular velocity for both bodies
NewtonBodyGetOmega(m_body0, &omega0[0]);
NewtonBodyGetOmega(m_body1, &omega1[0]);
// get angular velocity relative to the pin vector
w0 = omega0 % matrix0.m_front;
w1 = omega1 % matrix1.m_front;
// establish the gear equation.
relOmega = 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
relAccel = - 0.3f * relOmega / timestep;
// set the linear part of Jacobian 0 to zero
jacobian0[0] = 0.0f;
jacobian0[1] = 0.0f;
jacobian0[2] = 0.0f;
// set the angular part of Jacobian 0 pin vector
jacobian0[3] = matrix0.m_front[0];
jacobian0[4] = matrix0.m_front[1];
jacobian0[5] = matrix0.m_front[2];
// set the linear part of Jacobian 1 to zero
jacobian1[0] = 0.0f;
jacobian1[1] = 0.0f;
jacobian1[2] = 0.0f;
// set the angular part of Jacobian 1 pin vector
jacobian1[3] = matrix1.m_front[0];
jacobian1[4] = matrix1.m_front[1];
jacobian1[5] = matrix1.m_front[2];
// add a angular constraint
NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);
// set the desired angular acceleration between the two bodies
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
void dCustomBallAndSocket::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);
SubmitLinearRows(0x07, matrix0, matrix1);
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngleCos = coneDir1.DotProduct3(coneDir0);
dMatrix coneRotation(dGetIdentityMatrix());
dVector lateralDir(matrix0.m_up);
if (cosAngleCos < 0.9999f) {
lateralDir = coneDir1.CrossProduct(coneDir0);
dFloat mag2 = lateralDir.DotProduct3(lateralDir);
if (mag2 > 1.0e-4f) {
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit);
} else {
lateralDir = matrix0.m_up.Scale (-1.0f);
coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit);
}
}
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
dVector relOmega(omega0 - omega1);
// do twist angle calculations
dMatrix twistMatrix(matrix0 * (matrix1 * coneRotation).Inverse());
dFloat twistAngle = m_twistAngle.Update(dAtan2(twistMatrix[1][2], twistMatrix[1][1]));
if (m_options.m_option0) {
if ((m_minTwistAngle == 0.0f) && (m_minTwistAngle == 0.0f)) {
NewtonUserJointAddAngularRow(m_joint, -twistAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
} else {
if (m_options.m_option1) {
// TODO spring option
dAssert (0);
} else {
SubmitConstraintTwistLimits(matrix0, matrix1, relOmega, timestep);
}
}
} else if (m_options.m_option1) {
// TODO spring option
dAssert (0);
} else if (m_twistFriction > 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);
}
// do twist cone angle calculations
if (m_options.m_option2) {
if ((m_maxConeAngle == 0.0f)) {
dMatrix localMatrix(matrix0 * matrix1.Inverse());
dVector euler0;
dVector euler1;
localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);
NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, -euler0[2], &matrix1[2][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
} else {
if (m_options.m_option3) {
// TODO spring option
dAssert(0);
} else {
dFloat jointOmega = relOmega.DotProduct3(lateralDir);
dFloat currentAngle = dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)));
dFloat coneAngle = currentAngle + jointOmega * timestep;
if (coneAngle >= m_maxConeAngle) {
//dQuaternion rot(lateralDir, coneAngle);
//dVector frontDir(rot.RotateVector(coneDir1));
//dVector upDir(lateralDir.CrossProduct(frontDir));
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
const dFloat invtimestep = 1.0f / timestep;
const dFloat speed = 0.5f * (m_maxConeAngle - currentAngle) * invtimestep;
const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
NewtonUserJointSetRowAcceleration(m_joint, stopAccel);
} else if (m_coneFriction != 0) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
}
}
}
} else if (m_options.m_option3) {
// TODO spring option
dAssert(0);
} else if (m_coneFriction > 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
}
}
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 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 CustomUniversal::SubmitConstraints (dFloat timestep, int threadIndex)
{
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);
// get the pin fixed to the first body
const dVector& dir0 = matrix0.m_front;
// get the pin fixed to the second body
const dVector& dir1 = matrix1.m_up;
// construct an orthogonal coordinate system with these two vectors
dVector dir2 (dir0 * dir1);
dir2 = dir2.Scale (1.0f / dSqrt (dir2 % dir2));
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir0[0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir1[0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir2[0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
dVector dir3 (dir2 * dir0);
dVector q0 (p0 + dir3.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1 (p1 + dir1.Scale(MIN_JOINT_PIN_LENGTH));
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &dir0[0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// check is the joint limit are enable
if (m_limit_0_On) {
sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
angle = dAtan2 (sinAngle, cosAngle);
if (angle < m_minAngle_0) {
dFloat relAngle;
relAngle = angle - m_minAngle_0;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle > m_maxAngle_0) {
dFloat relAngle;
relAngle = angle - m_maxAngle_0;
// 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);
}
// check is the joint limit motor is enable
} else if (m_angularMotor_0_On) {
dFloat relOmega;
dFloat relAccel;
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
relOmega = (omega0 - omega1) % matrix0.m_front;
relAccel = m_angularAccel_0 - m_angularDamp_0 * relOmega;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
// if limit are enable ...
if (m_limit_1_On) {
sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up;
cosAngle = matrix0.m_front % matrix1.m_front;
angle = dAtan2 (sinAngle, cosAngle);
if (angle < m_minAngle_1) {
dFloat relAngle;
relAngle = angle - m_minAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle > m_maxAngle_1) {
dFloat relAngle;
relAngle = angle - m_maxAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
} else if (m_angularMotor_1_On) {
dFloat relOmega;
dFloat relAccel;
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
relOmega = (omega0 - omega1) % matrix1.m_up;
relAccel = m_angularAccel_1 - m_angularDamp_1 * relOmega;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix1.m_up[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
}