//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;
}
// public void CollisionResponce(Rigidbody grb,float e=0.5F) {
// //coefficient of restitution: ratio of speed after/before
//
//
// //
// // vector_3 Velocity = Configuration.CMVelocity +
// // CrossProduct(Configuration.AngularVelocity,R);
// //
// // real ImpulseNumerator = -(r(1) + Body.CoefficientOfRestitution) *
// // DotProduct(Velocity,CollisionNormal);
// //
// // real ImpulseDenominator = Body.OneOverMass +
// // DotProduct(CrossProduct(Configuration.InverseWorldInertiaTensor *
// // CrossProduct(R,CollisionNormal),R),
// // CollisionNormal);
// //
// // vector_3 Impulse = (ImpulseNumerator/ImpulseDenominator) * CollisionNormal;
// //
// // // apply impulse to primary quantities
// // Configuration.CMVelocity += Body.OneOverMass * Impulse;
// // Configuration.AngularMomentum += CrossProduct(R,Impulse);
// //
// // // compute affected auxiliary quantities
// // Configuration.AngularVelocity = Configuration.InverseWorldInertiaTensor *
// // Configuration.AngularMomentum;
//
// float ma = grb.mass;
// //manually set first
//
// Vector3 n = hitNormal;
//
// // grb.velocity =Vector3.zero;
// // grb.angularVelocity = Vector3.zero;
// // grb.Sleep ();
//
// // print (grb.velocity);
//
// Vector3 vai = grb.velocity;
//
// Vector3 wai = grb.angularVelocity;
//
// Vector3 ita = grb.inertiaTensor;
//
// Quaternion itra = grb.inertiaTensorRotation;
//
// Matrix3f Qa = Matrix3f.rotation (grb.inertiaTensorRotation);
// Matrix3f QaT = Qa.Transposed ();
//
//
// Matrix3f Ia = new Matrix3f (ita);
// // print (Aa+" ita:"+ita+" itra:"+itra+" Qa:"+Qa+" QaT:"+QaT);
// // Matrix3f Ia = Qa * Aa * QaT;
// Ia.print();
// Matrix3f IaInverse = Ia.inverse();
// print ("Pos:"+grb.position+" CM:" + grb.centerOfMass);
//
// Vector3 ra = grb.position - grb.worldCenterOfMass;
// Vector3 velocity = vai + Vector3.Cross (wai, ra);
// float impulsenumerator = -(e + 1) * Vector3.Dot (velocity, n);
// float impulsedenominator = 1 / ma + Vector3.Dot (Vector3.Cross (IaInverse * Vector3.Cross (ra, n), ra), n);
// Vector3 J = (impulsenumerator / impulsedenominator) * n;
// Vector3 vaii = grb.velocity;
// print ((1 / ma) * J);
// grb.velocity += (1/ma)*J;
// grb.angularVelocity = IaInverse * Vector3.Cross (ra,J);
// Debug.Log ("initial is"+vaii+"final is"+grb.velocity);
//
//
//
//
// //// IaInverse.print ();
// // Vector3 normal = n.normalized;
// // Vector3 angularVelChangea = normal; // start calculating the change in abgular rotation of a
// // angularVelChangea=Vector3.Cross(angularVelChangea,ra);
// //
// // Vector3 vaLinDueToR = Vector3.Cross(IaInverse*angularVelChangea, ra); // calculate the linear velocity of collision point on a due to rotation of a
// // float scalar = 1/ma + Vector3.Dot(vaLinDueToR,normal);
// //
// //
// // float Jmod = -(e+1)*(vai).magnitude/scalar;
// // Vector3 J = normal*Jmod;
// // Vector3 vaf = vai - J*(1/ma);
// // grb.velocity = vaf;
// // Debug.Log ("initial is"+vai+"final is"+vaf);
// // grb.angularVelocity = wai - angularVelChangea;
// }
public void CollisionResponce(Rigidbody grb, Rigidbody trb, float e = 0.5F)
{
float mb = trb.mass;
float ma = grb.mass;
//manually set first
Vector3 n = hitNormal;
Vector3 vai = grb.velocity;
Vector3 vbi = trb.velocity;
Vector3 wai = grb.angularVelocity;
Vector3 wbi = trb.angularVelocity;
Vector3 ita = grb.inertiaTensor;
Vector3 itb = trb.inertiaTensor;
Matrix3f Qa = Matrix3f.rotation(grb.inertiaTensorRotation);
Matrix3f QaT = Qa.Transposed();
Matrix3f Qb = Matrix3f.rotation(trb.inertiaTensorRotation);
Matrix3f QbT = Qb.Transposed();
Matrix3f Aa = new Matrix3f(ita);
Matrix3f Ab = new Matrix3f(itb);
Matrix3f Ia = Qa * Aa * QaT;
Matrix3f Ib = Qb * Ab * QbT;
Vector3 ra = grb.position - grb.worldCenterOfMass;
Vector3 rb = trb.position - trb.worldCenterOfMass;
Matrix3f IaInverse = Ia.inverse();
Vector3 normal = n.normalized;
Vector3 angularVelChangea = normal; // start calculating the change in abgular rotation of a
angularVelChangea = Vector3.Cross(angularVelChangea, ra);
Vector3 vaLinDueToR = Vector3.Cross(IaInverse * angularVelChangea, ra); // calculate the linear velocity of collision point on a due to rotation of a
float scalar = 1 / ma + Vector3.Dot(vaLinDueToR, normal);
Matrix3f IbInverse = Ib.inverse();
Vector3 angularVelChangeb = normal; // start calculating the change in abgular rotation of b
angularVelChangeb = Vector3.Cross(angularVelChangeb, rb);
Vector3 vbLinDueToR = Vector3.Cross(IbInverse * angularVelChangeb, rb); // calculate the linear velocity of collision point on b due to rotation of b
scalar += 1 / mb + Vector3.Dot(vbLinDueToR, normal);
float Jmod = (e + 1) * (vai - vbi).magnitude / scalar;
Vector3 J = normal * Jmod;
Vector3 vaf = vai - J * (1 / ma);
grb.velocity = vaf;
Debug.Log("initial is" + vai + "final is" + vaf);
trb.velocity = vbi - J * (1 / mb);
grb.angularVelocity = wai - angularVelChangea;
trb.angularVelocity = wbi - angularVelChangeb;
}
