void CustomVehicleControllerComponentTrackSkidSteering::Update (dFloat timestep) { // get the vehicle body state and the angular velocity along its up vector const CustomVehicleControllerBodyStateChassis& state = m_controller->m_chassisState; dFloat omega = state.m_omega % state.m_matrix[1]; dFloat omegaSign = dSign(omega); dFloat torque = m_steeringTorque * m_param - omega * omega * omegaSign * m_differencialTurnRate; // dTrace (("%f %f\n", torque, omega)); for (dList<CustomVehicleControllerBodyStateTire*>::dListNode* node = m_steeringTires.GetFirst(); node; node = node->GetNext()) { CustomVehicleControllerBodyStateTire& tire = *node->GetInfo(); const dMatrix& tireMatrix = tire.GetMatrix(); const dMatrix& tireLocalMatrix = tire.GetLocalMatrix(); dFloat torqueSign = -dSign (tireLocalMatrix[0] % tireLocalMatrix[3]); tire.SetTorque (tire.GetTorque () + tireMatrix[0].Scale (torque * torqueSign)); } }
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 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 CustomVehicleControllerComponentSteering::Update (dFloat timestep) { dFloat angle = m_maxAngle * m_param; if ((m_akermanWheelBaseWidth == 0.0f) || (dAbs (angle) < (2.0f * 3.141592f / 180.0f))) { for (dList<CustomVehicleControllerBodyStateTire*>::dListNode* node = m_steeringTires.GetFirst(); node; node = node->GetNext()) { CustomVehicleControllerBodyStateTire& tire = *node->GetInfo(); tire.m_steeringAngle = angle; } } else { dAssert (dAbs (angle) >= (2.0f * 3.141592f / 180.0f)); dFloat posit = m_akermanAxelSeparation / dTan (dAbs (angle)); dFloat sign = dSign (angle); dFloat leftAngle = sign * dAtan2 (m_akermanAxelSeparation, posit + m_akermanWheelBaseWidth); dFloat righAngle = sign * dAtan2 (m_akermanAxelSeparation, posit - m_akermanWheelBaseWidth); for (dList<CustomVehicleControllerBodyStateTire*>::dListNode* node = m_steeringTires.GetFirst(); node; node = node->GetNext()) { CustomVehicleControllerBodyStateTire& tire = *node->GetInfo(); tire.m_steeringAngle = (sign * tire.m_locationSign > 0.0f) ? leftAngle: righAngle; } } }
//dVector dMatrix::GetEulerAngles (dEulerAngleOrder order) const void dMatrix::GetEulerAngles (dVector & euler0, dVector & euler1, dEulerAngleOrder order) const { int a0 = (order >> 8) & 3; int a1 = (order >> 4) & 3; int a2 = (order >> 0) & 3; const dMatrix & matrix = *this; // Assuming the angles are in radians. if (matrix[a0][a2] > 0.99995f) { dFloat picth0 = 0.0f; dFloat yaw0 = -3.141592f * 0.5f; dFloat roll0 = - dAtan2 (matrix[a2][a1], matrix[a1][a1]); euler0[a0] = picth0; euler0[a1] = yaw0; euler0[a2] = roll0; euler1[a0] = picth0; euler1[a1] = yaw0; euler1[a2] = roll0; } else if (matrix[a0][a2] < -0.99995f) { dFloat picth0 = 0.0f; dFloat yaw0 = 3.141592f * 0.5f; dFloat roll0 = dAtan2 (matrix[a2][a1], matrix[a1][a1]); euler0[a0] = picth0; euler0[a1] = yaw0; euler0[a2] = roll0; euler1[a0] = picth0; euler1[a1] = yaw0; euler1[a2] = roll0; } else { //euler[a0] = -dAtan2(-matrix[a1][a2], matrix[a2][a2]); //euler[a1] = -dAsin ( matrix[a0][a2]); //euler[a2] = -dAtan2(-matrix[a0][a1], matrix[a0][a0]); dFloat yaw0 = -dAsin ( matrix[a0][a2]); dFloat yaw1 = 3.141592f - yaw0; dFloat sign0 = dSign (dCos (yaw0)); dFloat sign1 = dSign (dCos (yaw1)); dFloat picth0 = dAtan2 (matrix[a1][a2] * sign0, matrix[a2][a2] * sign0); dFloat picth1 = dAtan2 (matrix[a1][a2] * sign1, matrix[a2][a2] * sign1); dFloat roll0 = dAtan2 (matrix[a0][a1] * sign0, matrix[a0][a0] * sign0); dFloat roll1 = dAtan2 (matrix[a0][a1] * sign1, matrix[a0][a0] * sign1); if (yaw1 > 3.141592f) yaw1 -= 2.0f * 3.141592f; euler0[a0] = picth0; euler0[a1] = yaw0; euler0[a2] = roll0; euler1[a0] = picth1; euler1[a1] = yaw1; euler1[a2] = roll1; } euler0[3] = dFloat (0.0f); euler1[3] = dFloat (0.0f); #ifdef _DEBUG if (order == m_pitchYawRoll) { dMatrix m0 (dPitchMatrix (euler0[0]) * dYawMatrix (euler0[1]) * dRollMatrix (euler0[2])); dMatrix m1 (dPitchMatrix (euler1[0]) * dYawMatrix (euler1[1]) * dRollMatrix (euler1[2])); for (int i = 0; i < 3; i ++) { for (int j = 0; j < 3; j ++) { dFloat error = dAbs (m0[i][j] - matrix[i][j]); dAssert (error < 5.0e-2f); error = dAbs (m1[i][j] - matrix[i][j]); dAssert (error < 5.0e-2f); } } } #endif }
void CustomControlledBallAndSocket::SubmitConstraints (dFloat timestep, int threadIndex) { dMatrix matrix0; dMatrix matrix1; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix (matrix0, matrix1); // Restrict the movement on the pivot point along all tree orthonormal direction NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]); #if 0 dVector euler0; dVector euler1; dMatrix localMatrix (matrix0 * matrix1.Inverse()); localMatrix.GetEulerAngles(euler0, euler1); AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch); AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch); if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) { euler0 = euler1; } dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f); for (int i = 0; i < 3; i ++) { dFloat error = m_targetAngles[i] - euler[i]; if (dAbs (error) > (0.125f * 3.14159213f / 180.0f) ) { dFloat angularStep = dSign(error) * m_angulaSpeed * timestep; if (angularStep > 0.0f) { if (angularStep > error) { angularStep = error * 0.5f; } } else { if (angularStep < error) { angularStep = error * 0.5f; } } euler[i] = euler[i] + angularStep; } } dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2])); dMatrix baseMatrix (p0y0r0 * matrix1); dMatrix rotation (matrix0.Inverse() * baseMatrix); dQuaternion quat (rotation); if (quat.m_q0 > dFloat (0.99995f)) { dVector euler0; dVector euler1; rotation.GetEulerAngles(euler0, euler1); NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]); NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]); NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]); } else { dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f))); NewtonUserJointAddAngularRow (m_joint, 2.0f * dAcos (quat.m_q0), &basis[0][0]); NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[1][0]); NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[2][0]); } #else matrix1 = m_targetRotation * matrix1; dQuaternion localRotation(matrix1 * matrix0.Inverse()); if (localRotation.DotProduct(m_targetRotation) < 0.0f) { localRotation.Scale(-1.0f); } dFloat angle = 2.0f * dAcos(localRotation.m_q0); dFloat angleStep = m_angulaSpeed * timestep; if (angleStep < angle) { dVector axis(dVector(localRotation.m_q1, localRotation.m_q2, localRotation.m_q3, 0.0f)); axis = axis.Scale(1.0f / dSqrt(axis % axis)); // localRotation = dQuaternion(axis, angleStep); } dVector axis (matrix1.m_front * matrix1.m_front); dVector axis1 (matrix1.m_front * matrix1.m_front); //dFloat sinAngle; //dFloat cosAngle; //CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle); //float xxxx = dAtan2(sinAngle, cosAngle); //float xxxx1 = dAtan2(sinAngle, cosAngle); dQuaternion quat(localRotation); if (quat.m_q0 > dFloat(0.99995f)) { // dAssert (0); /* dVector euler0; dVector euler1; rotation.GetEulerAngles(euler0, euler1); NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]); NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]); NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]); */ } else { dMatrix basis(dGrammSchmidt(dVector(quat.m_q1, quat.m_q2, quat.m_q3, 0.0f))); NewtonUserJointAddAngularRow(m_joint, -2.0f * dAcos(quat.m_q0), &basis[0][0]); NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]); NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]); } #endif }
void 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); } }