//TODO: cross linear and rotational public void Integrate() { //linear //set position first ns_position = position + m_linearVelocity * Time.fixedDeltaTime; //set the velocity ns_m_linearVelocity = m_linearVelocity + inv_mass * this.forces * Time.fixedDeltaTime; //set the acceleration //ns_m_linearAcceleration += forces * inv_mass; //rotational GK.Matrix3f skewedAngurlar = new GK.Matrix3f(); skewedAngurlar = skewedAngurlar.SkewSymmetry(m_AngularVelocity); ns_orientation = orientation + skewedAngurlar * Time.fixedDeltaTime * orientation; Debug.Log("Orientation"); ns_orientation.print(); ns_AngularMomentum = AngularMomentum + Time.fixedDeltaTime * Torque; GK.Matrix3f transposedOrientation = new GK.Matrix3f(); transposedOrientation = ns_orientation.Transposed(); ns_WorldInertiaInverseTensor = (ns_orientation * BodyInertiaInverseTensor) * transposedOrientation; Debug.Log("WorldInertia"); ns_WorldInertiaInverseTensor.print(); ns_orientation.OrthonormalizeOrientation(); ns_m_AngularVelocity = ns_WorldInertiaInverseTensor * ns_AngularMomentum; }
void Integrate() { for (int counter = 0; counter < NumberOfBodies; counter++) { //get the gameobject GameObject gameObject = (GameObject)allBodies[counter]; //get the rigidbody of the object RigidBody rigidBody = gameObject.GetComponent <RigidBody>(); //linear //set position first if (rigidBody.position != rigidBody.transform.position) { rigidBody.position = rigidBody.transform.position; } rigidBody.ns_position = rigidBody.position + rigidBody.m_linearVelocity * Time.fixedDeltaTime; //set the velocity rigidBody.ns_m_linearVelocity = rigidBody.m_linearVelocity + rigidBody.inv_mass * rigidBody.forces * Time.fixedDeltaTime; //set the acceleration //rigidBody.ns_m_linearAcceleration += rigidBody.forces * rigidBody.inv_mass; //rotational GK.Matrix3f skewedAngurlar = new GK.Matrix3f(); skewedAngurlar = skewedAngurlar.SkewSymmetry(rigidBody.m_AngularVelocity); rigidBody.ns_orientation = rigidBody.orientation + skewedAngurlar * Time.fixedDeltaTime * rigidBody.orientation; rigidBody.ns_AngularMomentum = rigidBody.AngularMomentum + Time.fixedDeltaTime * rigidBody.Torque; GK.Matrix3f transposedOrientation = new GK.Matrix3f(); transposedOrientation = rigidBody.ns_orientation.Transposed(); rigidBody.ns_WorldInertiaInverseTensor = (rigidBody.ns_orientation * rigidBody.BodyInertiaInverseTensor) * transposedOrientation; rigidBody.ns_orientation.OrthonormalizeOrientation(); rigidBody.ns_m_AngularVelocity = rigidBody.ns_WorldInertiaInverseTensor * rigidBody.ns_AngularMomentum; } }