void cPhysicsBodyNewton::AddImpulse(const cVector3f &a_vImpulse) { cVector3f vMassCentre = GetMassCentre(); if (vMassCentre != cVector3f(0,0,0)) { cVector3f vCentreOffset = cMath::MatrixMul(GetWorldMatrix().GetRotation(), vMassCentre); cVector3f vWorldPosition = GetWorldPosition() + vCentreOffset; NewtonBodyAddImpulse(m_pNewtonBody, a_vImpulse.v, vWorldPosition.v); } else NewtonBodyAddImpulse(m_pNewtonBody, a_vImpulse.v, GetWorldPosition().v); }
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 cPhysicsBodyNewton::AddImpulseAtPosition(const cVector3f &a_vImpulse, const cVector3f &a_vPos) { NewtonBodyAddImpulse(m_pNewtonBody, a_vImpulse.v, a_vPos.v); }