//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;
}
}
