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 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); dVector p0(matrix0.m_posit); dVector p1(matrix1.m_posit); dVector dir(p1 - p0); dFloat mag2 = dir % dir; dir = dir.Scale(1.0f / dSqrt(mag2)); dMatrix matrix(dGrammSchmidt(dir)); dFloat x = dSqrt(mag2) - m_distance; dVector com0; dVector com1; dVector veloc0; dVector veloc1; dMatrix body0Matrix; dMatrix body1Matrix; NewtonBody* const body0 = GetBody0(); NewtonBody* const body1 = GetBody1(); NewtonBodyGetCentreOfMass(body0, &com0[0]); NewtonBodyGetMatrix(body0, &body0Matrix[0][0]); NewtonBodyGetPointVelocity(body0, &p0[0], &veloc0[0]); NewtonBodyGetCentreOfMass(body1, &com1[0]); NewtonBodyGetMatrix(body1, &body1Matrix[0][0]); NewtonBodyGetPointVelocity(body1, &p1[0], &veloc1[0]); dFloat v((veloc0 - veloc1) % dir); dFloat a = (x - v * timestep) / (timestep * timestep); dVector r0((p0 - body0Matrix.TransformVector(com0)) * matrix.m_front); dVector r1((p1 - body1Matrix.TransformVector(com1)) * matrix.m_front); dFloat jacobian0[6]; dFloat jacobian1[6]; jacobian0[0] = matrix[0][0]; jacobian0[1] = matrix[0][1]; jacobian0[2] = matrix[0][2]; jacobian0[3] = r0[0]; jacobian0[4] = r0[1]; jacobian0[5] = r0[2]; jacobian1[0] = -matrix[0][0]; jacobian1[1] = -matrix[0][1]; jacobian1[2] = -matrix[0][2]; jacobian1[3] = -r1[0]; jacobian1[4] = -r1[1]; jacobian1[5] = -r1[2]; NewtonUserJointAddGeneralRow(m_joint, jacobian0, jacobian1); NewtonUserJointSetRowAcceleration(m_joint, a); }
dFloat CustomPlayerController::CalculateContactKinematics(const dVector& veloc, const NewtonWorldConvexCastReturnInfo* const contactInfo) const { dVector contactVeloc(0.0f, 0.0f, 0.0f, 0.0f) ; if (contactInfo->m_hitBody) { NewtonBodyGetPointVelocity (contactInfo->m_hitBody, contactInfo->m_point, &contactVeloc[0]); } const dFloat restitution = 0.0f; dVector normal (contactInfo->m_normal); dFloat reboundVelocMag = -((veloc - contactVeloc)% normal) * (1.0f + restitution); return (reboundVelocMag > 0.0f) ? reboundVelocMag : 0.0f; }
void CustomPlayerController::UpdateGroundPlane (dMatrix& matrix, const dMatrix& castMatrix, const dVector& dst, int threadIndex) { CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager(); NewtonWorld* const world = manager->GetWorld(); CustomControllerConvexRayFilter filter(m_body); NewtonWorldConvexRayCast (world, m_castingShape, &castMatrix[0][0], &dst[0], CustomControllerConvexRayFilter::Filter, &filter, CustomControllerConvexRayFilter::Prefilter, threadIndex); m_groundPlane = dVector (0.0f, 0.0f, 0.0f, 0.0f); m_groundVelocity = dVector (0.0f, 0.0f, 0.0f, 0.0f); if (filter.m_hitBody) { m_isJumping = false; dVector supportPoint (castMatrix.m_posit + (dst - castMatrix.m_posit).Scale (filter.m_intersectParam)); m_groundPlane = filter.m_hitNormal; m_groundPlane.m_w = - (supportPoint % filter.m_hitNormal); NewtonBodyGetPointVelocity (filter.m_hitBody, &supportPoint.m_x, &m_groundVelocity[0]); matrix.m_posit = supportPoint; matrix.m_posit.m_w = 1.0f; } }
void CustomPlayerController::UpdateGroundPlane (dMatrix& matrix, const dMatrix& castMatrix, const dVector& dst, int threadIndex) { CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager(); NewtonWorld* const world = manager->GetWorld(); NewtonWorldConvexCastReturnInfo info; CustomControllerConvexRayFilter filter(m_body); dFloat param = 10.0f; int count = NewtonWorldConvexCast (world, &castMatrix[0][0], &dst[0], m_castingShape, ¶m, &filter, CustomControllerConvexCastPreFilter::Prefilter, &info, 1, threadIndex); m_groundPlane = dVector (0.0f); m_groundVelocity = dVector (0.0f); if (count && (param <= 1.0f)) { m_isJumping = false; dVector supportPoint (castMatrix.m_posit + (dst - castMatrix.m_posit).Scale (param)); m_groundPlane = dVector (info.m_normal[0], info.m_normal[1], info.m_normal[2], 0.0f); m_groundPlane.m_w = - supportPoint.DotProduct3(m_groundPlane); NewtonBodyGetPointVelocity (info.m_hitBody, &supportPoint.m_x, &m_groundVelocity[0]); matrix.m_posit = supportPoint; matrix.m_posit.m_w = 1.0f; } }
void CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep) { dVector com; dMatrix matrix; dVector omega0; dVector veloc0; dVector omega1; dVector veloc1; dVector pointVeloc; 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, 0.0f, 0.0f, 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); } } }
void dCustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex) { // check if this is an impulsive time step dMatrix matrix0(GetBodyMatrix()); dVector omega(0.0f); dVector com(0.0f); dVector pointVeloc(0.0f); const dFloat damp = 0.3f; dAssert (timestep > 0.0f); const dFloat invTimestep = 1.0f / timestep; // we not longer cap excessive angular velocities, it is left to the client application. NewtonBodyGetOmega(m_body0, &omega[0]); //cap excessive angular velocities dFloat mag2 = omega.DotProduct3(omega); if (mag2 > (m_omegaCap * m_omegaCap)) { omega = omega.Normalize().Scale(m_omegaCap); NewtonBodySetOmega(m_body0, &omega[0]); } // calculate the position of the pivot point and the Jacobian direction vectors, in global space. dVector relPosit(m_targetMatrix.m_posit - matrix0.m_posit); NewtonBodyGetPointVelocity(m_body0, &m_targetMatrix.m_posit[0], &pointVeloc[0]); for (int i = 0; i < 3; i ++) { // Restrict the movement on the pivot point along all tree orthonormal direction dFloat speed = pointVeloc.DotProduct3(m_targetMatrix[i]); dFloat dist = relPosit.DotProduct3(m_targetMatrix[i]) * damp; dFloat relSpeed = dist * invTimestep - speed; dFloat relAccel = relSpeed * invTimestep; NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &m_targetMatrix[i][0]); NewtonUserJointSetRowAcceleration(m_joint, relAccel); NewtonUserJointSetRowMinimumFriction(m_joint, -m_maxLinearFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_maxLinearFriction); } if (m_isSixdof) { dQuaternion rotation (matrix0.Inverse() * m_targetMatrix); if (dAbs (rotation.m_q0) < 0.99998f) { dMatrix rot (dGrammSchmidt(dVector (rotation.m_q1, rotation.m_q2, rotation.m_q3))); dFloat angle = 2.0f * dAcos(dClamp(rotation.m_q0, dFloat(-1.0f), dFloat(1.0f))); NewtonUserJointAddAngularRow (m_joint, angle, &rot.m_front[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &rot.m_up[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &rot.m_right[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); } else { NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); } } }