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 dCustomHinge::SubmitConstraintsFreeDof(int freeDof, const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep, int threadIndex)
{
dAssert (freeDof == 1);
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_friction = 0;
//m_limitsOn = false;
//m_setAsSpringDamper = 1;
//m_spring = 1000;
//m_damper = 10.0f;
//m_springDamperRelaxation = 0.97f;
if (m_setAsSpringDamper) {
//m_lastRowWasUsed = true;
NewtonUserJointAddAngularRow(m_joint, -GetPitch(), &matrix0.m_front[0]);
NewtonUserJointSetRowSpringDamperAcceleration(m_joint, m_springDamperRelaxation, m_spring, m_damper);
} else {
if (m_limitsOn) {
if (m_friction != 0.0f) {
SubmitConstraintsFrictionAndLimit(matrix0, matrix1, timestep);
} else {
SubmitConstraintsLimitsOnly(matrix0, matrix1, timestep);
}
} else {
if (m_friction != 0.0f) {
SubmitConstraintsFrictionOnly(matrix0, matrix1, timestep);
}
}
}
}
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 dCustomCorkScrew::SubmitConstraintSpringDamper(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
NewtonUserJointAddAngularRow(m_joint, -m_curJointAngle.GetAngle(), &matrix1.m_front[0]);
NewtonUserJointSetRowSpringDamperAcceleration(m_joint, m_angularSpringDamperRelaxation, m_angularSpring, m_angularDamper);
}
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 dCustomHinge::ApplySpringDamper (dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1)
{
m_lastRowWasUsed = true;
NewtonUserJointAddAngularRow(m_joint, 0.0f, &matrix1.m_front[0]);
NewtonUserJointSetRowSpringDamperAcceleration(m_joint, m_springDamperRelaxation, m_spring, m_damper);
}
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;
}
