void dgBilateralConstraint::SetSpringDamperAcceleration (dgInt32 index, dgContraintDescritor& desc, dgFloat32 spring, dgFloat32 damper)
{
if (desc.m_timestep > dgFloat32 (0.0f)) {
dgAssert (m_body1);
const dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0;
const dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1;
dgVector veloc0 (m_body0->m_veloc);
dgVector omega0 (m_body0->m_omega);
dgVector veloc1 (m_body1->m_veloc);
dgVector omega1 (m_body1->m_omega);
//dgFloat32 relPosit = (p1Global - p0Global) % jacobian0.m_linear + jointAngle;
dgFloat32 relPosit = desc.m_penetration[index];
dgFloat32 relVeloc = - (veloc0 % jacobian0.m_linear + veloc1 % jacobian1.m_linear + omega0 % jacobian0.m_angular + omega1 % jacobian1.m_angular);
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
dgFloat32 dt = desc.m_timestep;
dgFloat32 ks = dgAbsf (spring);
dgFloat32 kd = dgAbsf (damper);
dgFloat32 ksd = dt * ks;
dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
dgFloat32 den = dt * kd + dt * ksd;
dgFloat32 accel = num / (dgFloat32 (1.0f) + den);
// desc.m_jointStiffness[index] = - den / DG_PSD_DAMP_TOL ;
desc.m_jointStiffness[index] = - dgFloat32 (1.0f) - den / DG_PSD_DAMP_TOL;
SetMotorAcceleration (index, accel, desc);
}
}
void rayPickerManager::PreUpdate(dFloat timestep)
{
// all of the work will be done here;
dNewton::ScopeLock scopelock (&m_lock);
if (m_pickedBody) {
if (m_pickedBody->GetType() == dNewtonBody::m_dynamic) {
newtonDynamicBody* const body = (newtonDynamicBody*)m_pickedBody;
dFloat invTimeStep = 1.0f / timestep;
Matrix matrix (body->GetMatrix());
Vec4 omega0 (body->GetOmega());
Vec4 veloc0 (body->GetVeloc());
Vec4 peekPosit (m_localpHandlePoint * matrix);
Vec4 peekStep (m_globalTarget - peekPosit);
Vec4 pointVeloc (body->GetPointVeloc (peekPosit));
Vec4 deltaVeloc (peekStep * (m_stiffness * invTimeStep) - pointVeloc);
for (int i = 0; i < 3; i ++) {
Vec4 veloc (0.0f, 0.0f, 0.0f, 0.0f);
veloc[i] = deltaVeloc[i];
body->ApplyImpulseToDesiredPointVeloc (peekPosit, veloc);
}
// damp angular velocity
Vec4 omega1 (body->GetOmega());
Vec4 veloc1 (body->GetVeloc());
omega1 = omega1 * (0.9f);
// restore body velocity and angular velocity
body->SetVeloc(veloc0);
body->SetOmega(omega0);
// convert the delta velocity change to a external force and torque
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
body->GetMassAndInertia (mass, Ixx, Iyy, Izz);
matrix.setTrans(Vec3(0.0f, 0.0f, 0.0f));
Vec4 relOmega (omega1 - omega0);
relOmega = matrix.preMult(relOmega);
Vec4 angularMomentum (Ixx, Iyy, Izz, 0.0f);
angularMomentum = componentMultiply (relOmega, angularMomentum);
angularMomentum = matrix.postMult(angularMomentum);
Vec4 torque (angularMomentum * invTimeStep);
body->AddTorque(torque);
Vec4 relVeloc (veloc1 - veloc0);
Vec4 force (relVeloc * (mass * invTimeStep));
body->AddForce (force);
} else {
dAssert (0);
}
}
}
dgFloat32 dgBilateralConstraint::CalculateSpringDamperAcceleration (
dgInt32 index,
const dgContraintDescritor& desc,
dgFloat32 jointAngle,
const dgVector& p0Global,
const dgVector& p1Global,
dgFloat32 springK,
dgFloat32 springD)
{
dgFloat32 accel = 0.0f;
if (desc.m_timestep > dgFloat32 (0.0f)) {
dgAssert (m_body1);
const dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0;
const dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1;
dgVector veloc0 (m_body0->m_veloc);
dgVector omega0 (m_body0->m_omega);
dgVector veloc1 (m_body1->m_veloc);
dgVector omega1 (m_body1->m_omega);
dgFloat32 relPosit = (p1Global - p0Global) % jacobian0.m_linear + jointAngle;
dgFloat32 relVeloc = - (veloc0 % jacobian0.m_linear + veloc1 % jacobian1.m_linear + omega0 % jacobian0.m_angular + omega1 % jacobian1.m_angular);
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
dgFloat32 dt = desc.m_timestep;
dgFloat32 ks = springK;
dgFloat32 kd = springD;
dgFloat32 ksd = dt * ks;
dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
accel = num / den;
}
return accel;
}
void CustomSlider::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
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]);
// three rows to restrict rotation around around the parent coordinate system
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front), &matrix1.m_front[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]);
// calculate position and speed
dVector veloc0(0.0f);
dVector veloc1(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;
m_lastRowWasUsed = false;
SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
}
void MSP::PointToPoint::submit_constraints(const NewtonJoint* joint, dFloat timestep, int thread_index) {
MSP::Joint::JointData* joint_data = reinterpret_cast<MSP::Joint::JointData*>(NewtonJointGetUserData(joint));
PointToPointData* cj_data = reinterpret_cast<PointToPointData*>(joint_data->m_cj_data);
dFloat inv_timestep = 1.0f / timestep;
dMatrix matrix0, matrix1;
MSP::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1);
dVector p0(matrix0.m_posit + matrix0.m_right.Scale(cj_data->m_start_distance));
const dVector& p1 = matrix1.m_posit;
dVector veloc0(0.0f);
dVector veloc1(0.0f);
NewtonBodyGetVelocity(joint_data->m_child, &veloc0[0]);
if (joint_data->m_parent != nullptr)
NewtonBodyGetVelocity(joint_data->m_parent, &veloc1[0]);
dVector rel_veloc(veloc0 - veloc1);
dFloat stiffness = 0.999f - (1.0f - joint_data->m_stiffness_ratio * cj_data->m_strength) * Joint::DEFAULT_STIFFNESS_RANGE;
if (cj_data->m_mode == 0) {
dVector dir(p0 - p1);
cj_data->m_cur_distance = Util::get_vector_magnitude(dir);
if (cj_data->m_cur_distance > M_EPSILON)
Util::scale_vector(dir, 1.0f / cj_data->m_cur_distance);
else {
dir.m_x = 0.0f;
dir.m_y = 0.0f;
dir.m_z = 1.0f;
}
dFloat offset = cj_data->m_cur_distance - cj_data->m_start_distance * cj_data->m_controller;
dFloat dir_veloc = rel_veloc.DotProduct3(dir);
dFloat des_accel = cj_data->m_accel * -offset - dir_veloc * inv_timestep * cj_data->m_damp;
NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &dir[0]);
NewtonUserJointSetRowAcceleration(joint, des_accel);
NewtonUserJointSetRowStiffness(joint, stiffness);
}
else {
dVector offset(matrix1.UntransformVector(p0));
cj_data->m_cur_distance = Util::get_vector_magnitude(offset);
offset.m_z -= cj_data->m_start_distance * cj_data->m_controller;
dVector loc_vel(matrix1.UnrotateVector(rel_veloc));
dVector des_accel(offset.Scale(-cj_data->m_accel) - loc_vel.Scale(inv_timestep * cj_data->m_damp));
NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix1.m_front[0]);
NewtonUserJointSetRowAcceleration(joint, des_accel.m_x);
NewtonUserJointSetRowStiffness(joint, stiffness);
NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix1.m_up[0]);
NewtonUserJointSetRowAcceleration(joint, des_accel.m_y);
NewtonUserJointSetRowStiffness(joint, stiffness);
NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix1.m_right[0]);
NewtonUserJointSetRowAcceleration(joint, des_accel.m_z);
NewtonUserJointSetRowStiffness(joint, stiffness);
}
}
void dCustomPulley::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
dVector veloc0(0.0f);
dVector veloc1(0.0f);
dFloat jacobian0[6];
dFloat jacobian1[6];
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// set the linear part of Jacobian 0 to translational pin vector
dVector dir0 (matrix0.m_front.Scale (1.0f/m_gearRatio));
const dVector& dir1 = matrix1.m_front;
jacobian0[0] = dir0.m_x;
jacobian0[1] = dir0.m_y;
jacobian0[2] = dir0.m_z;
jacobian0[3] = 0.0f;
jacobian0[4] = 0.0f;
jacobian0[5] = 0.0f;
jacobian1[0] = dir1.m_x;
jacobian1[1] = dir1.m_y;
jacobian1[2] = dir1.m_z;
jacobian1[3] = 0.0f;
jacobian1[4] = 0.0f;
jacobian1[5] = 0.0f;
// calculate the angular velocity for both bodies
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
// get angular velocity relative to the pin vector
dFloat w0 = veloc0.DotProduct3(dir0);
dFloat w1 = veloc1.DotProduct3(dir1);
dFloat relVeloc = w0 + w1;
dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
dFloat relAccel = -0.5f * relVeloc * invTimestep;
// add a angular constraint
NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);
// set the desired angular acceleration between the two bodies
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
void CustomSlider::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], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
// three rows to restrict rotation around around the parent coordinate system
dFloat sinAngle;
dFloat cosAngle;
CalculatePitchAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_front[0]);
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]);
// calculate position and speed
dVector veloc0(0.0f, 0.0f, 0.0f, 0.0f);
dVector veloc1(0.0f, 0.0f, 0.0f, 0.0f);
if (m_body0) {
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
}
if (m_body1) {
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
}
m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix1.m_front;
m_speed = (veloc0 - veloc1) % matrix1.m_front;
// if limit are enable ...
m_hitLimitOnLastUpdate = false;
if (m_limitsOn) {
if (m_posit < m_minDist) {
// indicate that this row hit a limit
m_hitLimitOnLastUpdate = true;
// get a point along the up vector and set a constraint
const dVector& p0 = matrix0.m_posit;
dVector p1 (p0 + matrix0.m_front.Scale (m_minDist - m_posit));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
} else if (m_posit > m_maxDist) {
// indicate that this row hit a limit
m_hitLimitOnLastUpdate = true;
// get a point along the up vector and set a constraint
const dVector& p0 = matrix0.m_posit;
dVector p1 (p0 + matrix0.m_front.Scale (m_maxDist - m_posit));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else {
/*
// uncomment this for a slider with friction
// take any point on body0 (origin)
const dVector& p0 = matrix0.m_posit;
dVector veloc0;
dVector veloc1;
dVector omega1;
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
NewtonBodyGetOmega(m_body1, &omega1[0]);
// this assumes the origin of the bodies the matrix pivot are the same
veloc1 += omega1 * (matrix1.m_posit - p0);
dFloat relAccel;
relAccel = ((veloc1 - veloc0) % matrix0.m_front) / timestep;
#define MaxFriction 10.0f
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction (m_joint, -MaxFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, MaxFriction);
*/
}
}
}
void CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep)
{
dMatrix matrix;
dVector com(0.0f);
dVector omega0(0.0f);
dVector veloc0(0.0f);
dVector omega1(0.0f);
dVector veloc1(0.0f);
dVector pointVeloc(0.0f);
const dFloat stiffness = 0.3f;
const dFloat angularDamp = 0.95f;
dFloat invTimeStep = 1.0f / timestep;
NewtonWorld* const world = NewtonBodyGetWorld (body);
NewtonWorldCriticalSectionLock (world, 0);
// calculate the desired impulse
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonBodyGetOmega (body, &omega0[0]);
NewtonBodyGetVelocity (body, &veloc0[0]);
NewtonBodyGetPointVelocity (body, &pointOnBodyInGlobalSpace[0], &pointVeloc[0]);
dVector deltaVeloc (targetPositionInGlobalSpace - pointOnBodyInGlobalSpace);
deltaVeloc = deltaVeloc.Scale (stiffness * invTimeStep) - pointVeloc;
for (int i = 0; i < 3; i ++) {
dVector veloc (0.0f);
veloc[i] = deltaVeloc[i];
NewtonBodyAddImpulse (body, &veloc[0], &pointOnBodyInGlobalSpace[0]);
}
// damp angular velocity
NewtonBodyGetOmega (body, &omega1[0]);
NewtonBodyGetVelocity (body, &veloc1[0]);
omega1 = omega1.Scale (angularDamp);
// restore body velocity and angular velocity
NewtonBodySetOmega (body, &omega0[0]);
NewtonBodySetVelocity(body, &veloc0[0]);
// convert the delta velocity change to a external force and torque
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
dVector angularMomentum (Ixx, Iyy, Izz);
angularMomentum = matrix.RotateVector (angularMomentum.CompProduct(matrix.UnrotateVector(omega1 - omega0)));
dVector force ((veloc1 - veloc0).Scale (mass * invTimeStep));
dVector torque (angularMomentum.Scale(invTimeStep));
NewtonBodyAddForce(body, &force[0]);
NewtonBodyAddTorque(body, &torque[0]);
// make sure the body is unfrozen, if it is picked
NewtonBodySetSleepState (body, 0);
NewtonWorldCriticalSectionUnlock (world);
}
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);
}
}
}
