public Matrix3f(Matrix3f rm)
{
for (int i = 0; i < 9; i++)
{
m_elements[i] = rm.getElement(i);
}
}
//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;
}
// Use this for initialization
//define the intital values here
public RigidBody()
{
inv_mass = 1.0f / this.Mass;
//position = transform.position;
orientation = new GK.Matrix3f();
m_linearVelocity = new Vector3();
m_AngularVelocity = new Vector3();
Torque = new Vector3();
AngularMomentum = new Vector3();
forces = new Vector3();
//LinearMomentum = new Vector3();
m_AngularAccelration = new Vector3();
WorldInertiaInverseTensor = new GK.Matrix3f();
BodyInertiaInverseTensor = new GK.Matrix3f();
//position = transform.position;
ns_orientation = new GK.Matrix3f();
ns_m_linearVelocity = new Vector3();
ns_m_AngularVelocity = new Vector3();
ns_Torque = new Vector3();
ns_AngularMomentum = new Vector3();
ns_forces = new Vector3();
//ns_LinearMomentum = new Vector3();
ns_m_AngularAccelration = new Vector3();
ns_WorldInertiaInverseTensor = new GK.Matrix3f();
}
public bool intersect(MRay ray, MHit hit, float tmin)
{
Vector3 R0 = ray.origin;
Vector3 Rd = ray.direction;
Vector3 e1 = b - a;
Vector3 e2 = c - a;
//R0-a=beta*e1+gamma*e2-t*Rd
Matrix3f A = new Matrix3f(-e1, -e2, Rd, true);
Matrix3f betaM = new Matrix3f(a - R0, -e2, Rd, true);
Matrix3f gammaM = new Matrix3f(-e1, a - R0, Rd, true);
Matrix3f tM = new Matrix3f(-e1, -e2, a - R0, true);
float beta = betaM.determinant() / A.determinant();
float gamma = gammaM.determinant() / A.determinant();
float result_t = tM.determinant() / A.determinant();
float alpha = 1.0f - beta - gamma;
if (beta + gamma > 1 || beta < 0 || gamma < 0)
{
return(false);
}
if (result_t >= tmin && result_t < hit.t)
{
Vector3 newNormal = (alpha * normals[0] + beta * normals[1] + gamma * normals[2]).normalized;
hit.set(result_t, newNormal);
return(true);
}
return(false);
}
//code adapted from https://www.learnopencv.com/rotation-matrix-to-euler-angles/
public Matrix3f QuaterniontoMatrix3(Quaternion q)
{
Matrix3f m = new Matrix3f();
float sqw = q.w * q.w;
float sqx = q.x * q.x;
float sqy = q.y * q.y;
float sqz = q.z * q.z;
float inverse = 1 / (sqw + sqy + sqz + sqw);
m.setMELEMENT(0, 0, (sqx - sqy - sqz + sqw) * inverse);
m.setMELEMENT(1, 1, (-sqx + sqy - sqz + sqw) * inverse);
m.setMELEMENT(2, 2, (-sqx - sqy + sqz + sqw) * inverse);
float temp1 = q.x * q.y;
float temp2 = q.z * q.w;
m.setMELEMENT(1, 0, 2.0f * (temp1 + temp2) * inverse);
m.setMELEMENT(0, 1, 2.0f * (temp1 - temp2) * inverse);
float temp3 = q.x * q.z;
float temp4 = q.y * q.w;
m.setMELEMENT(2, 0, 2.0f * (temp3 - temp4) * inverse);
m.setMELEMENT(0, 2, 2.0f * (temp1 + temp2) * inverse);
float temp5 = q.y * q.z;
float temp6 = q.x * q.w;
m.setMELEMENT(2, 1, 2.0f * (temp5 + temp6) * inverse);
m.setMELEMENT(1, 2, 2.0f * (temp5 - temp6) * inverse);
return(m);
}
public Matrix3f SkewSymmetry(Vector3 v)
{
Matrix3f m = new Matrix3f();
m.m_elements[GetIndex(0, 0)] = 0.0f; m.m_elements[GetIndex(0, 1)] = -v.z; m.m_elements[GetIndex(0, 2)] = v.y;
m.m_elements[GetIndex(1, 0)] = v.z; m.m_elements[GetIndex(1, 1)] = 0.0f; m.m_elements[GetIndex(1, 2)] = -v.x;
m.m_elements[GetIndex(2, 0)] = -v.y; m.m_elements[GetIndex(2, 1)] = v.x; m.m_elements[GetIndex(2, 2)] = 0.0f;
return(m);
}
public Matrix3f Identity()
{
Matrix3f m = new Matrix3f();
m.getElements()[GetIndex(0, 0)] = 1.0f;
m.getElements()[GetIndex(1, 1)] = 1.0f;
m.getElements()[GetIndex(2, 2)] = 1.0f;
return(m);
}
// static
public Matrix3f Ones()
{
Matrix3f m = new Matrix3f();
for (int i = 0; i < 9; ++i)
{
m.getElements()[i] = 1;
}
return(m);
}
// 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;
}
public Matrix3f Transposed()
{
Matrix3f m = new Matrix3f();
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
m.getElements()[GetIndex(j, i)] = this.getElements()[GetIndex(i, j)];
}
}
return(m);
}
public void CollisionResponce(Rigidbody grb, Rigidbody trb, Vector3 v, Vector3 hitNormal, float e = 0.3F)
{
//coefficient of restitution: ratio of speed after/before
float ma = grb.mass;
float mb = trb.mass;
Vector3 n = hitNormal.normalized;
Vector3 vai = grb.velocity;
Vector3 vbi = trb.velocity;
Vector3 wai = grb.angularVelocity;
Vector3 wbi = trb.angularVelocity;
Vector3 ita = grb.inertiaTensor;
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:"+v+" CM:" + grb.centerOfMass);
// //need to change, this is point of impact
Vector3 ra = (v - grb.worldCenterOfMass);
Vector3 itb = trb.inertiaTensor;
Matrix3f Ib = new Matrix3f(itb);
Matrix3f IbInverse = Ib.inverse();
Vector3 rb = impactpt - trb.worldCenterOfMass;
Vector3 normal = n.normalized;
Vector3 angularVelChangea = Vector3.Cross(normal, ra);
Vector3 vaLinDueToR = Vector3.Cross(IaInverse * angularVelChangea, ra); // calculate the linear velocity of collision point on a due to rotation of a
Vector3 angularVelChangeb = Vector3.Cross(normal, rb);
Vector3 vbLinDueToR = Vector3.Cross(IbInverse * angularVelChangeb, rb);
//
float scalar = 1 / ma + 1 / mb + Vector3.Dot(vaLinDueToR, normal) + 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;
trb.velocity = -vbi + J * (1 / mb);
Vector3 deltawa = IaInverse * Vector3.Cross(J, ra);
Vector3 deltawb = IbInverse * Vector3.Cross(J, rb);
grb.angularVelocity = wai - deltawa;
trb.angularVelocity = wbi - deltawb;
}
// RIGIDBODY FUNCTIONS
public void SetVariables()
{
position = ns_position;
//m_linearAcceleration = ns_m_linearAcceleration;
//position = transform.position;
orientation = ns_orientation;
m_linearVelocity = ns_m_linearVelocity;
m_AngularVelocity = ns_m_AngularVelocity;
Torque = ns_Torque;
AngularMomentum = ns_AngularMomentum;
//LinearMomentum = ns_LinearMomentum;
m_AngularAccelration = ns_AngularMomentum;
WorldInertiaInverseTensor = ns_WorldInertiaInverseTensor;
}
public static Matrix3f operator +(Matrix3f x, Matrix3f y)
{
Matrix3f product = new Matrix3f(); // zeroes
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
product.m_elements[product.GetIndex(i, j)] = (x.getElement(i, j) + y.getElement(i, j));
//product.setElement(i, k, product.getElement(i, k) + x.getElement(i, j) * y.getElement(j, k));
}
}
return(product);
}
public void CollisionResponce(RigidBody grb, Rigidbody trb, Vector3 v, Vector3 hitNormal, float e = 0.5F)
{
//coefficient of restitution: ratio of speed after/before
Vector3 targetNormal = hitNormal.normalized;
Vector3 n = hitNormal.normalized;
float ma = grb.Mass;
float mb = trb.mass;
Vector3 vai = grb.m_linearVelocity;
Vector3 vbi = trb.velocity;
Vector3 wai = grb.m_AngularVelocity;
Vector3 wbi = trb.angularVelocity;
Vector3 ita = grb.GetInertiaTensor();
Matrix3f IaInverse = grb.WorldInertiaInverseTensor;
Vector3 ra = (v - grb.ns_position);
Vector3 itb = trb.inertiaTensor;
Matrix3f Ib = new Matrix3f(itb);
Matrix3f IbInverse = Ib.inverse();
Vector3 rb = v - trb.worldCenterOfMass;
Vector3 normal = -n.normalized;
Vector3 angularVelChangea = Vector3.Cross(normal, ra); // start calculating the change in angular rotation of a
Vector3 vaLinDueToR = Vector3.Cross(IaInverse * angularVelChangea, ra); // calculate the linear velocity of collision point on a due to rotation of a
Vector3 angularVelChangeb = Vector3.Cross(normal, rb);
Vector3 vbLinDueToR = Vector3.Cross(IbInverse * angularVelChangeb, rb);
// calculate the linear velocity of collision point on a due to rotation of a
float scalar = 1 / ma + 1 / mb + Vector3.Dot(vaLinDueToR, normal) + Vector3.Dot(vbLinDueToR, normal);
float Jmod = -(e + 1) * (vai - vbi).magnitude / scalar;
Vector3 J = normal * Jmod;
Vector3 vaf = vai - J * grb.inv_mass;
grb.m_linearVelocity = vaf;
trb.velocity = -vbi + J * (1 / mb);
Vector3 deltawa = IaInverse * Vector3.Cross(J, ra);
Vector3 deltawb = IbInverse * Vector3.Cross(J, rb);
grb.m_AngularVelocity = wai - deltawa;
trb.angularVelocity = wbi - deltawb;
}
// UNITY DEFINED FUNCTIONS
void Start()
{
//initialise the initial conditions
/*
* InverseInertiaTensor
*
*/
ns_position = transform.position;
ns_orientation = orientation.QuaterniontoMatrix3(transform.rotation);
float halfX = GetComponent <Renderer>().bounds.extents.x;
float halfY = GetComponent <Renderer>().bounds.extents.y;
float halfZ = GetComponent <Renderer>().bounds.extents.z;
BodyInertiaInverseTensor.setMELEMENT(0, 0, 12f / (Mass * (halfY * halfY + halfZ * halfZ)));
BodyInertiaInverseTensor.setMELEMENT(1, 1, 12f / (Mass * (halfX * halfX + halfZ * halfZ)));
BodyInertiaInverseTensor.setMELEMENT(2, 2, 12f / (Mass * (halfX * halfX + halfY * halfY)));
}
public static Matrix3f operator *(Matrix3f x, float y)
{
Matrix3f product = new Matrix3f(); // zeroes
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
for (int k = 0; k < 3; ++k)
{
product.m_elements[product.GetIndex(i, k)] *= y;
//product.setElement(i, k, product.getElement(i, k) + x.getElement(i, j) * y.getElement(j, k));
}
}
}
return(product);
}
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;
}
}
public Quaternion matrixs3fToQuaternion(Matrix3f m)
{
Quaternion q = new Quaternion();
float trace = m.m_elements[GetIndex(0, 0)] + m.m_elements[GetIndex(1, 1)] + m.m_elements[GetIndex(2, 2)];
if (trace > 0)
{
float s = 0.5f / Mathf.Sqrt(trace + 1.0f);
q.w = 0.25f / s;
q.x = (m.m_elements[GetIndex(2, 1)] - m.m_elements[GetIndex(1, 2)]) * s;
q.y = (m.m_elements[GetIndex(0, 2)] - m.m_elements[GetIndex(2, 0)]) * s;
q.z = (m.m_elements[GetIndex(1, 0)] - m.m_elements[GetIndex(0, 1)]) * s;
}
else
{
if (m.m_elements[GetIndex(0, 0)] > m.m_elements[GetIndex(1, 1)] && m.m_elements[GetIndex(0, 0)] > m.m_elements[GetIndex(2, 2)])
{
float s = 2.0f * Mathf.Sqrt(1.0f + m.m_elements[GetIndex(0, 0)] - m.m_elements[GetIndex(1, 1)] - m.m_elements[GetIndex(2, 2)]);
q.w = (m.m_elements[GetIndex(2, 1)] - m.m_elements[GetIndex(1, 2)]) / s;
q.x = 0.25f * s;
q.y = (m.m_elements[GetIndex(0, 1)] + m.m_elements[GetIndex(1, 0)]) / s;
q.z = (m.m_elements[GetIndex(0, 2)] + m.m_elements[GetIndex(2, 0)]) / s;
}
else if (m.m_elements[GetIndex(1, 1)] > m.m_elements[GetIndex(2, 2)])
{
float s = 2.0f * Mathf.Sqrt(1.0f + m.m_elements[GetIndex(1, 1)] - m.m_elements[GetIndex(0, 0)] - m.m_elements[GetIndex(2, 2)]);
q.w = (m.m_elements[GetIndex(0, 2)] - m.m_elements[GetIndex(2, 0)]) / s;
q.x = (m.m_elements[GetIndex(0, 1)] + m.m_elements[GetIndex(1, 0)]) / s;
q.y = 0.25f * s;
q.z = (m.m_elements[GetIndex(1, 2)] + m.m_elements[GetIndex(2, 1)]) / s;
}
else
{
float s = 2.0f * Mathf.Sqrt(1.0f + m.m_elements[GetIndex(2, 2)] - m.m_elements[GetIndex(0, 0)] - m.m_elements[GetIndex(1, 1)]);
q.w = (m.m_elements[GetIndex(1, 0)] - m.m_elements[GetIndex(0, 1)]) / s;
q.x = (m.m_elements[GetIndex(0, 2)] + m.m_elements[GetIndex(2, 0)]) / s;
q.y = (m.m_elements[GetIndex(1, 2)] + m.m_elements[GetIndex(2, 1)]) / s;
q.z = 0.25f * s;
}
}
return(q);
}
public void SetRotation(GK.Matrix3f Orientation)
{
float sy = Mathf.Sqrt(Orientation.getElement(0, 0) * Orientation.getElement(0, 0) + Orientation.getElement(1, 0) * Orientation.getElement(1, 0));
bool singular = sy < 1e-6; // If
float x, y, z;
if (!singular)
{
x = Mathf.Atan2(Orientation.getElement(2, 1), Orientation.getElement(2, 2));
y = Mathf.Atan2(-Orientation.getElement(2, 0), sy);
z = Mathf.Atan2(Orientation.getElement(1, 0), Orientation.getElement(0, 0));
}
else
{
x = Mathf.Atan2(-Orientation.getElement(1, 2), Orientation.getElement(1, 1));
y = Mathf.Atan2(-Orientation.getElement(2, 0), sy);
z = 0;
}
Debug.Log("euler" + new Vector3(x, y, z));
transform.Rotate(new Vector3(x, y, z));
}
public void CollisionResponce(RigidBody grb, Rigidbody trb, Vector3 v, Vector3 hitNormal, float e = 0.5F)
{
//coefficient of restitution: ratio of speed after/before
Vector3 targetNormal = hitNormal.normalized;
Vector3 n = hitNormal.normalized;
//Debug.Log("targetnormal");
//Debug.Log(targetNormal);
//Vector3 ra = v - grb.position;
//Debug.Log("ra");
//Debug.Log(ra);
//Vector3 Velocity = grb.m_linearVelocity + Vector3.Cross(grb.m_AngularVelocity, ra);
//Debug.Log("velocity");
//Debug.Log(Velocity);
//float ImpulseNumerator = -(1.0f + e) * Vector3.Dot(Velocity, targetNormal);
//Debug.Log("ImpulseNumerator");
//Debug.Log(ImpulseNumerator);
//float ImpulseDenominator = grb.inv_mass + Vector3.Dot(Vector3.Cross(grb.WorldInertiaInverseTensor * Vector3.Cross(ra, targetNormal), ra), hitNormal);
//Debug.Log("ImpulseDenominator");
//Debug.Log(ImpulseDenominator);
//Vector3 J = (ImpulseNumerator * 1.0f / ImpulseDenominator * 1.0f) * targetNormal;
//Debug.Log("J");
//Debug.Log(J);
//grb.m_linearVelocity += grb.inv_mass * J;
//grb.AngularMomentum += Vector3.Cross(ra, J);
//grb.m_AngularVelocity = grb.WorldInertiaInverseTensor * grb.AngularMomentum;
float ma = grb.Mass;
float mb = trb.mass;
Vector3 vai = grb.m_linearVelocity;
Vector3 vbi = trb.velocity;
Vector3 wai = grb.m_AngularVelocity;
Vector3 wbi = trb.angularVelocity;
Vector3 ita = grb.GetInertiaTensor();
Matrix3f IaInverse = grb.WorldInertiaInverseTensor;
//need to change, this is point of impact
Vector3 ra = (v - grb.ns_position);
Vector3 itb = trb.inertiaTensor;
Matrix3f Ib = new Matrix3f(itb);
Matrix3f IbInverse = Ib.inverse();
Vector3 rb = v - trb.worldCenterOfMass;
Vector3 normal = -n.normalized;
Vector3 angularVelChangea = Vector3.Cross(normal, ra); // start calculating the change in angular rotation of a
Vector3 vaLinDueToR = Vector3.Cross(IaInverse * angularVelChangea, ra); // calculate the linear velocity of collision point on a due to rotation of a
Vector3 angularVelChangeb = Vector3.Cross(normal, rb);
Vector3 vbLinDueToR = Vector3.Cross(IbInverse * angularVelChangeb, rb);
// calculate the linear velocity of collision point on a due to rotation of a
float scalar = 1 / ma + 1 / mb + Vector3.Dot(vaLinDueToR, normal) + Vector3.Dot(vbLinDueToR, normal);
float Jmod = -(e + 1) * (vai).magnitude / scalar;
Vector3 J = normal * Jmod;
Vector3 vaf = vai - J * grb.inv_mass;
grb.m_linearVelocity = vaf;
trb.velocity = -vbi + J * (1 / mb);
Vector3 deltawa = IaInverse * Vector3.Cross(J, ra);
Vector3 deltawb = IbInverse * Vector3.Cross(J, rb);
grb.m_AngularVelocity = wai - deltawa;
trb.angularVelocity = wbi - deltawb;
//float ma = grb.Mass;
//float mb = trb.Mass;
//Vector3 vai = grb.m_linearVelocity;
//Vector3 vbi = trb.m_linearVelocity;
//Vector3 wai = grb.m_AngularVelocity;
//Vector3 wbi = trb.m_AngularVelocity;
//Vector3 ita = grb.GetInertiaTensor();
//Matrix3f IaInverse = grb.WorldInertiaInverseTensor;
////need to change, this is point of impact
//Vector3 ra = (v - grb.ns_position);
////Vector3 itb = trb.World;
////Matrix3f Ib = new Matrix3f(itb);
//Matrix3f IbInverse = trb.WorldInertiaInverseTensor;
//Vector3 rb = v - trb.ns_position;
//Vector3 normal = n.normalized;
//Vector3 angularVelChangea = Vector3.Cross(normal, ra); // start calculating the change in angular rotation of a
//Vector3 vaLinDueToR = Vector3.Cross(IaInverse * angularVelChangea, ra); // calculate the linear velocity of collision point on a due to rotation of a
//Vector3 angularVelChangeb = Vector3.Cross(normal, rb);
//Vector3 vbLinDueToR = Vector3.Cross(IbInverse * angularVelChangeb, rb);
//// calculate the linear velocity of collision point on a due to rotation of a
//float scalar = 1 / ma + 1 / mb + Vector3.Dot(vaLinDueToR, normal) + Vector3.Dot(vbLinDueToR, normal);
//float Jmod = -(e + 1) * (vai).magnitude / scalar;
//Vector3 J = normal * Jmod;
//Vector3 vaf = vai - J * grb.inv_mass;
//grb.m_linearVelocity = vaf;
//trb.m_linearVelocity = -vbi + J * (1 / mb);
//Vector3 deltawa = IaInverse * Vector3.Cross(J, ra);
//Vector3 deltawb = IbInverse * Vector3.Cross(J, rb);
//grb.m_AngularVelocity = wai - deltawa;
//trb.m_AngularVelocity = wbi - deltawb;
}
