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);
}
}
