private void collisionDetection(ref logicalObject logObject)
{
if (logObject.Position.Y <= 1)
{
logObject.Position.Y = 1.0f;
//logObject.linearMomentum.Y *= -5f;
logObject.linearMomentum *= 0.9f;
logObject.angularMomentum *= 0.9f;
}
}
public void updatePhysics(ref logicalObject logObject, ref physicsMesh physMesh, bool intScene)
{
if (speed < 0) speed = 0f;
if (speed > 1) speed = 1f;
// Console.WriteLine("phys speed: " + speed);
if (speed != 0)
{
h = speed / 60f;
Cd = logObject.Cd;
oldPosition = logObject.Position;
oldRotation = logObject.Rotation;
oldAngularMomentum = logObject.angularMomentum;
oldLinearMomentum = logObject.linearMomentum;
oldAngularVelocity = logObject.angularVelocity;
oldRotationQuaternion = logObject.rotationQuaternion;
calculatePosition(ref logObject, physMesh.totalMass);
if (usingQuat)
calculateQuaternionRotation(ref logObject, physMesh.inertiaTensorInverse);
else calculateMatrixRotation(ref logObject, physMesh.inertiaTensorInverse);
if (intScene)
{
collisionDetection(ref logObject);
}
calculateMomentum(ref logObject, oldRotation, physMesh.numberOfPolygons, physMesh.polygonArray, physMesh.totalMass, oldAngularVelocity, oldLinearMomentum, oldPosition);
}
}
public void calculateQuaternionRotation(ref logicalObject logObject, Matrix3 inertiaTensorInverse)
{
angularVelocityQuaternion = new Quaternion();
tempQuaternion = new Quaternion();
angularVelocity = logObject.Rotation * inertiaTensorInverse * logObject.Rotation.Transverse() * logObject.angularMomentum;
angularVelocityStar.Star(angularVelocity);
angularVelocityQuaternion.W = 0.0f;
//Console.WriteLine("Quat");
angularVelocityQuaternion.X = angularVelocity.X;
angularVelocityQuaternion.Y = angularVelocity.Y;
angularVelocityQuaternion.Z = angularVelocity.Z;
tempQuaternion = Quaternion.Multiply(angularVelocityQuaternion, 0.5f);
tempQuaternion = Quaternion.Multiply(tempQuaternion, logObject.rotationQuaternion);
//Console.WriteLine(logObject.rotationQuaternion.Length());
logObject.rotationQuaternion += Quaternion.Multiply(tempQuaternion, h);
logObject.rotationQuaternion = Quaternion.Normalize(logObject.rotationQuaternion);
logObject.Rotation.QuaternionToMatrix3(logObject.rotationQuaternion);
logObject.angularVelocity = angularVelocity;
}
public void calculatePosition(ref logicalObject logObject, float totalMass)
{
//Console.WriteLine("Position: " + logObject.Position);
logObject.Position += h * logObject.linearMomentum / totalMass;
//Console.WriteLine("linear: " + logObject.linearMomentum);
}
public void calculateMomentum(ref logicalObject logObject, Matrix3 oldRotation, int numberOfPolygons, polygon[] polygonArray, float totalMass, Vector3 angularVelocity, Vector3 oldLinearMomentum, Vector3 oldPosition)
{
forceI.X = forceI.Y = forceI.Z = 0.0f;
totalForce.X = totalForce.Y = totalForce.Z = 0.0f;
torque.X = torque.Y = torque.Z = 0.0f;
//Console.WriteLine("Linear: " + oldLinearMomentum / totalMass);
//Console.WriteLine("ny");
for (int i = 0; i < numberOfPolygons; i++)
{
forceI = calculateCentroidForce(polygonArray[i], oldLinearMomentum / totalMass, oldRotation, angularVelocity, oldPosition);
totalForce += forceI;
torque += Vector3.Cross(oldRotation * polygonArray[i].centerOfMass, forceI);
//Console.WriteLine("hoho " + Vector3.Cross(oldRotation * polygonArray[i].centerOfMass, forceI));
}
//Console.WriteLine("torque " + torque);
//Console.WriteLine("Total Force: " + totalForce + " Length: " + totalForce.Length());
logObject.linearMomentum += h * totalForce;
//Console.WriteLine("Speed: " + logObject.linearMomentum.Length());
logObject.angularMomentum += h * torque;
}
public void calculateMatrixRotation(ref logicalObject logObject, Matrix3 inertiaTensorInverse)
{
angularVelocity = logObject.Rotation * inertiaTensorInverse * logObject.Rotation.Transverse() * logObject.angularMomentum;
angularVelocityStar.Star(angularVelocity);
logObject.Rotation += h * angularVelocityStar * logObject.Rotation;
logObject.Rotation = logObject.Rotation.GramSchmidt();
logObject.angularVelocity = angularVelocity;
//Console.WriteLine("Rot");
}