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(); }