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::SubmitConstraintsLimitsOnly(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, 0.0f); // 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, 0.0f); // m_lastRowWasUsed = true; } }
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 dCustomHinge::SubmitConstraints(dFloat timestep, int threadIndex) { dMatrix matrix0; dMatrix matrix1; dFloat sinAngle; dFloat cosAngle; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix(matrix0, matrix1); // Restrict the movement on the pivot point along all tree orthonormal direction NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); // two rows to restrict rotation around around the parent coordinate system NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); // the joint angle can be determined by getting the angle between any two non parallel vectors CalculateAngle (matrix1.m_up, matrix0.m_up, matrix1.m_front, sinAngle, cosAngle); m_curJointAngle.Update(cosAngle, sinAngle); // save the current joint Omega dVector omega0(0.0f); dVector omega1(0.0f); NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } m_jointOmega = (omega0 - omega1).DotProduct3(matrix1.m_front); m_lastRowWasUsed = false; if (m_setAsSpringDamper) { ApplySpringDamper (timestep, matrix0, matrix1); } else { SubmitConstraintsFreeDof (timestep, matrix0, matrix1); } }
void 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 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 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 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 PhyFixed::updateJoint(real timestep) { // If the joint is disabled, we don't do anything here if (!m_shared->enabled) { return; } // Updating the global matrices and the dof updateJointInfo(); #ifdef WORLDSIM_USE_NEWTON UNUSED_PARAM( timestep ); //--- Restrict the movement on the centre of the joint along all tree orthonormal direction NewtonUserJointAddLinearRow( m_priv->joint, &m_shared->globalMatrixChild.w_pos[0], &m_shared->globalMatrixParent.w_pos[0], &m_shared->globalMatrixParent.x_ax[0] ); NewtonUserJointSetRowStiffness( m_priv->joint, 1.0f ); NewtonUserJointAddLinearRow( m_priv->joint, &m_shared->globalMatrixChild.w_pos[0], &m_shared->globalMatrixParent.w_pos[0], &m_shared->globalMatrixParent.y_ax[0] ); NewtonUserJointSetRowStiffness( m_priv->joint, 1.0f ); NewtonUserJointAddLinearRow( m_priv->joint, &m_shared->globalMatrixChild.w_pos[0], &m_shared->globalMatrixParent.w_pos[0], &m_shared->globalMatrixParent.z_ax[0] ); NewtonUserJointSetRowStiffness( m_priv->joint, 1.0f ); //--- In order to constraint the rotation about X and Y axis of the joint //--- we use LinearRow (that are stronger) getting a point far from objects along //--- the Z axis. Doing this if the two object rotates about X or Y axes then //--- the difference between qChild and qParent augments and then Newton Engine will apply //--- a corresponding force (that applyied outside the centre of the object will become //--- a torque) that will blocks any rotation about X and Y axis real len = 5000.0; wVector qChild( m_shared->globalMatrixChild.w_pos + m_shared->globalMatrixChild.z_ax.scale(len) ); wVector qParent( m_shared->globalMatrixParent.w_pos + m_shared->globalMatrixParent.z_ax.scale(len) ); NewtonUserJointAddLinearRow( m_priv->joint, &qChild[0], &qParent[0], &m_shared->globalMatrixParent.x_ax[0] ); NewtonUserJointSetRowStiffness( m_priv->joint, 1.0 ); NewtonUserJointAddLinearRow( m_priv->joint, &qChild[0], &qParent[0], &m_shared->globalMatrixParent.y_ax[0] ); NewtonUserJointSetRowStiffness( m_priv->joint, 1.0 ); NewtonUserJointAddAngularRow( m_priv->joint, 0.0f, &m_shared->globalMatrixParent.z_ax[0] ); NewtonUserJointSetRowStiffness( m_priv->joint, 1.0f ); //--- In order to do the same with third axis (Z), I need others point along different axis /* qChild = wVector( m_shared->globalMatrixChild.w_pos + m_shared->globalMatrixChild.y_ax.scale(len) ); qParent = wVector( m_shared->globalMatrixParent.w_pos + m_shared->globalMatrixParent.y_ax.scale(len) ); NewtonUserJointAddLinearRow( m_priv->joint, &qChild[0], &qParent[0], &m_shared->globalMatrixParent.z_ax[0] ); NewtonUserJointSetRowStiffness( m_priv->joint, 1.0 );*/ //--- Retrive forces applied to the joint by the constraints Shared* const d = m_shared.getModifiableShared(); d->forceOnJoint.x = NewtonUserJointGetRowForce (m_priv->joint, 0); d->forceOnJoint.y = NewtonUserJointGetRowForce (m_priv->joint, 1); d->forceOnJoint.z = NewtonUserJointGetRowForce (m_priv->joint, 2); #endif }
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 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 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 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 CustomSlidingContact::SubmitConstraints (dFloat timestep, int threadIndex) { dMatrix matrix0; dMatrix matrix1; dFloat sinAngle; dFloat cosAngle; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix (matrix0, matrix1); // Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion dVector p0(matrix0.m_posit); dVector p1(matrix1.m_posit + matrix1.m_front.Scale((p0 - matrix1.m_posit) % matrix1.m_front)); NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_up[0]); NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_right[0]); // 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_up, matrix1_1.m_up, matrix1_1.m_front), &matrix1_1.m_front[0]); NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_up, matrix1_1.m_up, matrix1_1.m_right), &matrix1_1.m_right[0]); // the joint angle can be determined by getting the angle between any two non parallel vectors CalculateAngle(matrix1_1.m_front, matrix1.m_front, matrix1.m_up, sinAngle, cosAngle); m_curJointAngle.Update(cosAngle, sinAngle); dVector veloc0(0.0f, 0.0f, 0.0f, 0.0f); dVector veloc1(0.0f, 0.0f, 0.0f, 0.0f); dAssert(m_body0); NewtonBodyGetPointVelocity(m_body0, &matrix0.m_posit[0], &veloc0[0]); if (m_body1) { NewtonBodyGetPointVelocity(m_body1, &matrix1.m_posit[0], &veloc1[0]); } m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix1.m_front; m_speed = (veloc0 - veloc1) % matrix1.m_front; // if limit are enable ... if (m_limitsLinearOn) { if (m_posit < m_minLinearDist) { // get a point along the up vector and set a constraint dVector p (matrix1.m_posit + matrix1.m_front.Scale(m_minLinearDist)); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &p[0], &matrix1.m_front[0]); // allow the object to return but not to kick going forward NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f); } else if (m_posit > m_maxLinearDist) { dVector p(matrix1.m_posit + matrix1.m_front.Scale(m_maxLinearDist)); NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &p[0], &matrix1.m_front[0]); // allow the object to return but not to kick going forward NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f); } } if (m_limitsAngularOn) { dFloat angle1 = m_curJointAngle.GetAngle(); if (angle1 < m_minAngularDist) { dFloat relAngle = angle1 - m_minAngularDist; // 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_maxAngularDist) { dFloat relAngle = angle1 - m_maxAngularDist; // 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); } } }
void CustomHinge::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); // Restrict the movement on the pivot point along all tree orthonormal direction NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_front[0]); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]); // get a point along the pin axis at some reasonable large distance from the pivot dVector q0 (matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH)); dVector q1 (matrix1.m_posit + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH)); // two constraints row perpendicular to the pin vector NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]); NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]); // the joint angle can be determine by getting the angle between any two non parallel vectors dFloat angle; dFloat sinAngle; dFloat cosAngle; sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front; cosAngle = matrix0.m_up % matrix1.m_up; angle = m_curJointAngle.CalculateJointAngle (cosAngle, sinAngle); // if limit are enable ... if (m_limitsOn) { // the joint angle can be determine by getting the angle between any two non parallel vectors // sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front; // cosAngle = matrix0.m_up % matrix1.m_up; // angle = dAtan2 (sinAngle, cosAngle); // if (angle < m_minAngle) { if (angle < m_minAngle) { dFloat relAngle; // relAngle = angle - m_minAngle; relAngle = angle - m_minAngle; // the angle was clipped save the new clip limit m_curJointAngle.m_angle = m_minAngle; // 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_maxAngle) { dFloat relAngle; // relAngle = angle - m_maxAngle; relAngle = angle - m_maxAngle; // the angle was clipped save the new clip limit m_curJointAngle.m_angle = m_maxAngle; // 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); } } // save the current joint Omega 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]); } m_jointOmega = (omega0 - omega1) % matrix0.m_front; }
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 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); } }
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 MSNewton::CurvySlider::submit_constraints(const NewtonJoint* joint, dgFloat32 timestep, int thread_index) { JointData* joint_data = (JointData*)NewtonJointGetUserData(joint); CurvySliderData* cj_data = (CurvySliderData*)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); dVector location = matrix2.UntransformVector(matrix0.m_posit); dVector point, vector, min_pt, max_pt; dFloat distance, min_len, max_len; if (!c_calc_curve_data_at_location(cj_data, location, point, vector, distance, min_pt, max_pt, min_len, max_len)) { cj_data->cur_data_set = false; return; } point = matrix2.TransformVector(point); vector = matrix2.RotateVector(vector); min_pt = matrix2.TransformVector(min_pt); max_pt = matrix2.TransformVector(max_pt); cj_data->cur_point = point; cj_data->cur_vector = vector; cj_data->cur_tangent = (1.0f - dAbs(vector.m_z) < EPSILON) ? Y_AXIS * vector : Z_AXIS * vector; cj_data->cur_data_set = true; dFloat last_pos = cj_data->cur_pos; dFloat last_vel = cj_data->cur_vel; if (cj_data->loop) { dFloat diff1 = distance - cj_data->last_dist; dFloat diff2 = diff1 + (diff1 > 0 ? -cj_data->curve_len : cj_data->curve_len); if (dAbs(diff1) < dAbs(diff2)) cj_data->cur_pos += diff1; else cj_data->cur_pos += diff2; } else cj_data->cur_pos = distance; cj_data->cur_vel = (cj_data->cur_pos - last_pos) / timestep; cj_data->cur_accel = (cj_data->cur_vel - last_vel) / timestep; cj_data->last_dist = distance; dMatrix matrix3; Util::matrix_from_pin_dir(point, vector, matrix3); const dVector& p0 = matrix0.m_posit; const dVector& p1 = matrix3.m_posit; dVector p00(p0 + matrix0.m_right.Scale(MIN_JOINT_PIN_LENGTH)); dVector p11(p1 + matrix3.m_right.Scale(MIN_JOINT_PIN_LENGTH)); // Restrict movement on the pivot point along the normal and bi normal of the path. NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix3.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], &matrix3.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); // Align to curve if (cj_data->align) { NewtonUserJointAddLinearRow(joint, &p00[0], &p11[0], &matrix3.m_front[0]); if (joint_data->ctype == CT_ROBUST) NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint)); else NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); NewtonUserJointAddLinearRow(joint, &p00[0], &p11[0], &matrix3.m_up[0]); if (joint_data->ctype == CT_ROBUST) NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint)); else NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); } // Add linear friction or limits dFloat min_posit = matrix3.UntransformVector(min_pt).m_z; dFloat max_posit = matrix3.UntransformVector(max_pt).m_z; dFloat cur_posit = matrix3.UntransformVector(p0).m_z; dFloat margin = EPSILON + 0.01f * dAbs(cj_data->cur_vel); if (cur_posit < min_posit - margin || (cur_posit < min_posit - Joint::LINEAR_LIMIT_EPSILON && dAbs(min_len) < EPSILON && cj_data->loop == false)) { NewtonUserJointAddLinearRow(joint, &p0[0], &min_pt[0], &matrix3.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 (cur_posit > max_posit + margin || (cur_posit > max_posit + Joint::LINEAR_LIMIT_EPSILON && dAbs(max_len - cj_data->curve_len) < EPSILON && cj_data->loop == false)) { NewtonUserJointAddLinearRow(joint, &p0[0], &max_pt[0], &matrix3.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(matrix3.UntransformVector(matrix0.m_posit)); point.m_z = 0.0f; point = matrix3.TransformVector(point); NewtonUserJointAddLinearRow(joint, &point[0], &matrix3.m_posit[0], &matrix3.m_right[0]); dFloat power = cj_data->linear_friction * cj_data->controller; NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); } // Add angular friction or limits if (cj_data->rotate) { if (cj_data->align) { NewtonUserJointAddAngularRow(joint, 0.0f, &matrix3.m_right[0]); dFloat power = cj_data->angular_friction * cj_data->controller; NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); } else { dFloat cur_cone_angle_cos = matrix0.m_right % cj_data->last_dir; if (dAbs(cur_cone_angle_cos) < 0.99995f) { dVector lateral_dir = matrix0.m_right * cj_data->last_dir; Util::normalize_vector(lateral_dir); NewtonUserJointAddAngularRow(joint, 0.0f, &lateral_dir[0]); dFloat power = cj_data->angular_friction * cj_data->controller; NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); } } } else if (cj_data->align) { NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_front, matrix3.m_front, matrix3.m_right), &matrix3.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); } else { // 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 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); } cj_data->last_dir = matrix0.m_right; }