private void UpdateRigidBodyParameters(RigidBody o, float dt) { RigidBodyData data = o.RigidBodyData; if (data.Mass != 0) { data.Acceleration = data.Forces / data.Mass; } else { data.Acceleration = new MyVector(0, 0, 0); } data.Velocity.Add(data.Acceleration * dt); data.AngularAcceleration = data.InertiaInverse * (data.Moments - (data.AngularVelocity ^ (data.Inertia * data.AngularVelocity))); data.AngularVelocity += data.AngularAcceleration * dt; //data.Orientation += data.Orientation * (data.AngularVelocity * (dt / 2)); //data.Orientation.Normalize(); MyMatrix rot = data.Orientation.RotationMatrix; MyMatrix rotinv = rot.Inverse; data.InertiaInverseGlobal = rot * data.InertiaInverse * rotinv; data.VelocityBody = data.Velocity; data.VelocityBody.Rotate(~data.Orientation); data.Forces = new MyVector(0, 0, 0); data.Moments = new MyVector(0, 0, 0); o.RigidBodyData = data; }
private void CalculateRigidBodyForces(RigidBody o) { RigidBodyData data = o.RigidBodyData; //data.Forces = new MyVector(0, 0, 0); //data.Moments = new MyVector(0, 0, 0); if (doGravity) data.Forces = o.Mass * gravity; if (doLinearDrag) { data.Forces -= data.Velocity * air_density * (data.Velocity.Length * o.LinearDragCoefficient * o.BoundingSphereRadius * o.BoundingSphereRadius); } if (doAngularDrag) { data.Moments -= data.AngularVelocity * air_density * (data.AngularVelocity.Length * o.AngularDragCoefficient * o.BoundingSphereRadius * o.BoundingSphereRadius); data.Moments -= data.AngularVelocity * air_density * (o.AngularDragCoefficient * o.BoundingSphereRadius * o.BoundingSphereRadius); //constantDrag if (data.AngularVelocity.Length > 0) { data.Moments -= data.AngularVelocity * air_density * (data.AngularVelocity.Length * o.AngularDragCoefficient * o.BoundingSphereRadius * o.BoundingSphereRadius) / (float)Math.Sqrt(data.AngularVelocity.Length); } if (data.AngularVelocity.LengthSq < 0.00001f) data.AngularVelocity.X = data.AngularVelocity.Y = data.AngularVelocity.Z = 0; } o.RigidBodyData = data; }