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);
}
}
// Denna funktion initierar fysiken och gör dem beräkningar som är nödvändiga
// för att en simulering skall kunna genomföras.
public void initPhysics(ref physicsMesh physMesh)
{
// Ett pappers densitet i kg/m^3
physMesh.density = 780f + RandomClass.Next(3000);
p = 1.21f;
for (int i = 0; i < physMesh.numberOfPolygons; i++)
{
calculateCentroid(ref physMesh.polygonArray[i]);
calculatePolygonMass(ref physMesh.polygonArray[i], physMesh.density);
}
calculateObjectMass(ref physMesh);
calculateObjectCenterOfMass(ref physMesh);
calculateInertiaTensorInverse(ref physMesh);
physMesh.initiated = true;
}
public void calculateObjectCenterOfMass(ref physicsMesh physMesh)
{
Vector3 COM = new Vector3();
for (int i = 0; i < physMesh.polygonArray.Length; i++)
{
COM = COM + Vector3.Multiply(physMesh.polygonArray[i].centerOfMass, physMesh.polygonArray[i].mass);
}
COM = COM / physMesh.totalMass;
//Console.WriteLine("COM " + COM);
for (int i = 0; i < physMesh.polygonArray.Length; i++)
{
physMesh.polygonArray[i].vertices[0] = physMesh.polygonArray[i].vertices[0] - COM;
physMesh.polygonArray[i].vertices[1] = physMesh.polygonArray[i].vertices[1] - COM;
physMesh.polygonArray[i].vertices[2] = physMesh.polygonArray[i].vertices[2] - COM;
}
physMesh.centerOfMass = COM;
for (int i = 0; i < physMesh.numberOfPolygons; i++)
{
calculateCentroid(ref physMesh.polygonArray[i]);
}
}
public void calculateObjectMass(ref physicsMesh physMesh)
{
float totalMass = 0.0f;
for (int i = 0; i < physMesh.polygonArray.Length; i++)
{
totalMass += physMesh.polygonArray[i].mass;
}
physMesh.totalMass = totalMass;
//Console.WriteLine("total mass: " + totalMass);
}
public void calculateInertiaTensorInverse(ref physicsMesh physMesh)
{
Matrix3 Ibody = new Matrix3();
for (int i = 0; i < physMesh.polygonArray.Length; i++)
{
//Console.WriteLine("i: " + physMesh.polygonArray[i].mass);
Ibody.M11 = Ibody.M11 + physMesh.polygonArray[i].mass * ((float)Math.Pow(physMesh.polygonArray[i].centerOfMass.Y, 2.0) + (float)Math.Pow(physMesh.polygonArray[i].centerOfMass.Z, 2.0));
Ibody.M12 = Ibody.M12 + physMesh.polygonArray[i].mass * -1 * ((physMesh.polygonArray[i].centerOfMass.X) * (physMesh.polygonArray[i].centerOfMass.Y));
Ibody.M13 = Ibody.M13 + physMesh.polygonArray[i].mass * -1 * ((physMesh.polygonArray[i].centerOfMass.X) * (physMesh.polygonArray[i].centerOfMass.Z));
Ibody.M21 = Ibody.M21 + physMesh.polygonArray[i].mass * -1 * ((physMesh.polygonArray[i].centerOfMass.X) * (physMesh.polygonArray[i].centerOfMass.Y));
Ibody.M22 = Ibody.M22 + physMesh.polygonArray[i].mass * ((float)Math.Pow(physMesh.polygonArray[i].centerOfMass.X, 2.0) + (float)Math.Pow(physMesh.polygonArray[i].centerOfMass.Z, 2.0));
Ibody.M23 = Ibody.M23 + physMesh.polygonArray[i].mass * -1 * ((physMesh.polygonArray[i].centerOfMass.Y) * (physMesh.polygonArray[i].centerOfMass.Z));
Ibody.M31 = Ibody.M31 + physMesh.polygonArray[i].mass * -1 * ((physMesh.polygonArray[i].centerOfMass.X) * (physMesh.polygonArray[i].centerOfMass.Z));
Ibody.M32 = Ibody.M32 + physMesh.polygonArray[i].mass * -1 * ((physMesh.polygonArray[i].centerOfMass.Y) * (physMesh.polygonArray[i].centerOfMass.Y));
Ibody.M33 = Ibody.M33 + physMesh.polygonArray[i].mass * ((float)Math.Pow(physMesh.polygonArray[i].centerOfMass.X, 2.0) + (float)Math.Pow(physMesh.polygonArray[i].centerOfMass.Y, 2.0));
}
physMesh.inertiaTensorInverse = Ibody.Inverse();
}
