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 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);
}
}
}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
const dFloat angleError = GetMaxAngleError();
dFloat angle0 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up);
NewtonUserJointAddAngularRow(m_joint, angle0, &matrix1.m_up[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
if (dAbs(angle0) > angleError) {
const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle0 / (timestep * timestep);
NewtonUserJointSetRowAcceleration(m_joint, alpha);
}
dFloat angle1 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right);
NewtonUserJointAddAngularRow(m_joint, angle1, &matrix1.m_right[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
if (dAbs(angle1) > angleError) {
const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle1 / (timestep * timestep);
NewtonUserJointSetRowAcceleration(m_joint, alpha);
}
// the joint angle can be determined by getting the angle between any two non parallel vectors
m_curJointAngle.Update(-CalculateAngle(matrix0.m_up, matrix1.m_up, matrix1.m_front));
// 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 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 CustomBallAndSocketWithFriction::SubmitConstraints(dFloat timestep, int threadIndex)
{
CustomBallAndSocket::SubmitConstraints(timestep, threadIndex);
dVector omega0(0.0f, 0.0f, 0.0f, 0.0f);
dVector omega1(0.0f, 0.0f, 0.0f, 0.0f);
// get the omega vector
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
dVector relOmega(omega0 - omega1);
dFloat omegaMag = dSqrt(relOmega % relOmega);
if (omegaMag > 0.1f) {
// tell newton to used this the friction of the omega vector to apply the rolling friction
dMatrix basis(dGrammSchmidt(relOmega));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
// calculate the acceleration to stop the ball in one time step
dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[0][0]);
NewtonUserJointSetRowAcceleration(m_joint, -omegaMag * invTimestep);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
} else {
// when omega is too low this is correct but the small angle approximation theorem.
dMatrix basis(dGetIdentityMatrix());
for (int i = 0; i < 3; i++) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[i][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
}
}
}
TEST_F(test_core, transition_INPSS)
{
sas::String<M1> alpha1("ALPHA"), omega1("OMEGA");
// define network
entry. link(term, pound, atInvoke, 1);
entry. link(term, lowercase, atNegative, 3);
entry. link(term, letters, atPositive, 2);
entry. link(exit, alpha1, atSimple, 4);
entry. link(exit, omega1, atSimple, 5);
exit. link(term, end, atSimple, 6);
// test parsing
EXPECT_EQ( "3", parse(entry, "#Alpha") );
EXPECT_EQ( "3", parse(entry, "#Omega") );
EXPECT_EQ( "3", parse(entry, "#alpha") );
EXPECT_EQ( "3", parse(entry, "#omega") );
EXPECT_EQ( "5 | 1 4 6", parse(entry, "#ALPHA") );
EXPECT_EQ( "5 | 1 5 6", parse(entry, "#OMEGA") );
}
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);
}
}
}
}
// [[Rcpp::interfaces(cpp)]]
double pr(int choice,int m,int chg,double add0,double add1,double* mGam,double* mOm1,double* mOm2,int* mRnew,int* mR,int mg){
arma::icolvec R(mR,mg,false);
arma::icolvec Rnew(mRnew,mg,false);
arma::colvec gamma(mGam,m,false);
arma::colvec omega1(mOm1,m,false);
arma::colvec omega2(mOm2,m,false);
double out=0;
double add=((R[chg+choice]==0)?add0:add1);
double addnew=((Rnew[chg+choice]==0)?add0:add1);
if(choice==0){
out=out+pRedge(gamma[choice],omega2[choice],Rnew[chg+choice],Rnew[chg+choice+1],addnew);
out=out-pRedge(gamma[choice],omega2[choice],R[chg+choice],R[chg+choice+1],add);
}
if(choice==(m-1)){
out=out+pRedge(gamma[choice],omega1[choice],Rnew[chg+choice],Rnew[chg+choice-1],addnew);
out=out-pRedge(gamma[choice],omega1[choice],R[chg+choice],R[chg+choice-1],add);
}
if(choice>0 && choice<(m-1)){
out=out+pRmiddle(gamma[choice],omega1[choice],omega2[choice],Rnew[chg+choice],Rnew[chg+choice-1],Rnew[chg+choice+1],addnew);
out=out-pRmiddle(gamma[choice],omega1[choice],omega2[choice],R[chg+choice],R[chg+choice-1],R[chg+choice+1],add);
}
return out;
}
void FBBaseFunction< Dimension, Scalar >::compute(
const Vector& x, const Vector& y, Vector& fb,
Matrix& dFb_dx, Matrix& dFb_dy )
{
const unsigned d = x.rows() ;
// see [Daviet et al 2011], Appendix A.1
Vector z2(d) ;
z2[0] = x.squaredNorm() + y.squaredNorm() ;
Traits::tp( z2 ) = x[0] * Traits::tp( x ) + y[0] * Traits::tp( y ) ;
const Scalar nz2t = Traits::tp( z2 ).norm() ;
Vector omega1(d), omega2(d) ;
omega1[0] = omega2[0] = .5 ;
if( NumTraits< Scalar >::isZero( nz2t ) )
{
Traits::tp( omega1 ).setZero() ;
omega1[1] = -.5 ;
Traits::tp( omega2 ).setZero() ;
omega2[1] = .5 ;
} else {
Traits::tp( omega1 ) = -.5* Traits::tp( z2 ) / nz2t ;
Traits::tp( omega2 ) = .5* Traits::tp( z2 ) / nz2t ;
}
const Scalar rlambda1 = std::sqrt( std::max( (Scalar) 0, z2[0] - 2*nz2t ) ) ;
const Scalar rlambda2 = std::sqrt( z2[0] + 2*nz2t ) ;
const Vector z = rlambda1 * omega1 + rlambda2 * omega2 ;
fb = x + y - z ;
if( JacobianAsWell )
{
const Matrix Id( Matrix::Identity( d, d ) ) ;
if( NumTraits< Scalar >::isZero( rlambda2 ) )
{
// x = y = 0
dFb_dx.setZero( d,d ) ;
dFb_dy.setZero( d,d ) ;
} else {
if( NumTraits< Scalar >::isZero( rlambda1 ) )
{
const Scalar izn = 1. / ( x[0]*x[0] + y[0]*y[0] ) ;
dFb_dx = ( 1. - x[0]*izn ) * Id ;
dFb_dy = ( 1. - y[0]*izn ) * Id ;
} else {
const Scalar det = rlambda1 * rlambda2 ;
Matrix L, invLz(d,d) ;
invLz(0, 0) = z[0] ;
Traits::ntb( invLz ) = -Traits::tp( z ).transpose() ;
Traits::tnb( invLz ) = -Traits::tp( z ) ;
Traits::ttb( invLz ) =
( det * Traits::TgMatrix::Identity( d-1, d-1 ) +
Traits::tp( z ) * Traits::tp( z ).transpose() ) / z[0] ;
invLz /= det ;
L = x[0] * Id ;
Traits::ntb( L ) = Traits::tp( x ).transpose() ;
Traits::tnb( L ) = Traits::tp( x ) ;
dFb_dx.setIdentity( d,d ) ;
dFb_dx.noalias() -= invLz * L ;
L = y[0] * Id ;
Traits::ntb( L ) = Traits::tp( y ).transpose() ;
Traits::tnb( L ) = Traits::tp( y ) ;
dFb_dy.setIdentity( d,d ) ;
dFb_dy.noalias() -= invLz * L ;
}
}
}
}
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 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);
}
}
// 2. 四次方程中所调用的三次方程
double cubic_equation(double a3,double b3,double c3,double d3)
{
double p,q,delta;
double M,N;
double y0;
complex<double> temp1,temp2;
complex<double> x1,x2,x3;
complex<double> y1,y2;
if(a3==0)
{
quadratic_equation(b3,c3,d3,y1,y2);
x1 = y1;
x2 = y2;
x3 = 0;
} else {
p = -1.0/3*pow((b3*1.0/a3),2.0)+c3*1.0/a3;
q = 2.0/27*pow((b3*1.0/a3),3.0)-1.0/3*b3*c3/(a3*a3)+d3*1.0/a3;
// p = -1/3*(b3/a3)^2+c3/a3;
// q = 2/27*(b3/a3)^3-1/3*b3*c3/(a3*a3)+d3/a3;
//化成 y^3+p*y+q=0 形式；
delta = pow((q/2.0),2.0)+pow((p/3.0),3.0);
//判别式 delta = (q/2)^2+(p/3)^3;
//cout<<" delta>0, 有一个实根和两个复根；delta=0,有三个实根；delta<0,有三个不等的实根"<<endl;
//cout<<" delta = " << delta << endl;
//cout="">& lt;<" please output the discriminant delta="<<& lt;endl; delta="">0,有一个实根和两个复根；
//delta=0,有三个实根；
////delta<0,有三个不等的实根。
complex<double> omega1(-1.0/2, sqrt(3.0)/2.0);
complex<double> omega2(-1.0/2, -sqrt(3.0)/2.0);
complex<double> yy(b3/(3.0*a3),0.0);
M = -q/2.0;
if(delta<0)
{
N = sqrt(fabs(delta));
complex<double> s1(M,N);
complex<double> s2(M,-N);
x1 = (pow(s1,(1.0/3))+pow(s2,(1.0/3)))-yy;
x2 = (pow(s1,(1.0/3))*omega1+pow(s2,(1.0/3))*omega2)-yy;
x3 = (pow(s1,(1.0/3))*omega2+pow(s2,(1.0/3))*omega1)-yy;
} else {
N = sqrt(delta);
complex<double> f1(M+N,0);
complex<double> f2(M-N,0);
if(M+N >= 0)
temp1 = pow((f1),1.0/3);
else
temp1 = -norm(pow(sqrt(f1),1.0/3));
if(M-N >= 0)
temp2 = pow((f2),1.0/3);
else
temp2 = -norm(pow(sqrt(f2),1.0/3));
x1 = temp1+temp2-yy;
x2 = omega1*temp1+omega2*temp2-yy;
x3 = omega2*temp1+omega1*temp2-yy;
}
}
y0 = real(x1);
return y0;
//y0 为所调用的三次方程的返回值；
}
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 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 CustomCorkScrew::SubmitConstrainst ()
{
dFloat dist;
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
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], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_right[0]);
// get a point along the pin axis at some reasonable large distance from the pivot
dVector q0 (p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1 (p1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// two constraints row perpendiculars to the hinge pin
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]);
// if limit are enable ...
if (m_limitsOn) {
dist = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
if (dist < m_minDist) {
// 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_maxDist) {
// 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);
}
}
if (m_angularmotorOn) {
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 - 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 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 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;
}
