private void AddBodyToDrawList(RigidBody rb) { if (rb.Tag is BodyTag && ((BodyTag)rb.Tag) == BodyTag.DontDrawMe) { return; } bool isCompoundShape = rb.Shape is CompoundShape; if (!isCompoundShape) { AddShapeToDrawList(rb.Shape, rb.Orientation, rb.Position); } else { var cShape = rb.Shape as CompoundShape; var orientation = rb.Orientation; var position = rb.Position; foreach (var ts in cShape.Shapes) { var pos = ts.Position; var ori = ts.Orientation; JVector.Transform(ref pos, ref orientation, out pos); JVector.Add(ref pos, ref position, out pos); JMatrix.Multiply(ref ori, ref orientation, out ori); AddShapeToDrawList(ts.Shape, ori, pos); } } }
/// <summary> /// Recalculates the axis aligned bounding box and the inertia /// values in world space. /// </summary> public virtual void Update() { if (isParticle) { this.inertia = JMatrix.Zero; this.invInertia = this.invInertiaWorld = JMatrix.Zero; this.invOrientation = this.orientation = JMatrix.Identity; this.boundingBox = shape.boundingBox; JVector.Add(ref boundingBox.Min, ref this.position, out boundingBox.Min); JVector.Add(ref boundingBox.Max, ref this.position, out boundingBox.Max); angularVelocity.MakeZero(); } else { // Given: Orientation, Inertia JMatrix.Transpose(ref orientation, out invOrientation); this.Shape.GetBoundingBox(ref orientation, out boundingBox); JVector.Add(ref boundingBox.Min, ref this.position, out boundingBox.Min); JVector.Add(ref boundingBox.Max, ref this.position, out boundingBox.Max); if (!isStatic) { JMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld); JMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld); } } }
private void DrawBody(RigidBody rb) { if (rb.Tag is bool && ((bool)rb.Tag) == true) { return; } bool isCompoundShape = (rb.Shape is CompoundShape); if (!isCompoundShape) { DrawShape(rb.Shape, rb.Orientation, rb.Position); } else { CompoundShape cShape = rb.Shape as CompoundShape; JMatrix orientation = rb.Orientation; JVector position = rb.Position; foreach (var ts in cShape.Shapes) { JVector pos = ts.Position; JMatrix ori = ts.Orientation; JVector.Transform(ref pos, ref orientation, out pos); JVector.Add(ref pos, ref position, out pos); JMatrix.Multiply(ref ori, ref orientation, out ori); DrawShape(ts.Shape, ori, pos); } } }
/// <summary> /// Recalculates the axis aligned bounding box and the inertia /// values in world space. /// </summary> public virtual void Update() { // Given: Orientation, Inertia JMatrix.Transpose(ref orientation, out invOrientation); this.Shape.GetBoundingBox(ref orientation, out boundingBox); JVector.Add(ref boundingBox.Min, ref this.position, out boundingBox.Min); JVector.Add(ref boundingBox.Max, ref this.position, out boundingBox.Max); if (!isStatic) { JMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld); JMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld); } }
/// <summary> /// Called once before iteration starts. /// </summary> /// <param name="timestep">The 5simulation timestep</param> public override void PrepareForIteration(double timestep) { effectiveMass = body1.invInertiaWorld + body2.invInertiaWorld; softnessOverDt = softness / timestep; effectiveMass.M11 += softnessOverDt; effectiveMass.M22 += softnessOverDt; effectiveMass.M33 += softnessOverDt; JMatrix.Inverse(ref effectiveMass, out effectiveMass); JMatrix orientationDifference; JMatrix.Multiply(ref initialOrientation1, ref initialOrientation2, out orientationDifference); JMatrix.Transpose(ref orientationDifference, out orientationDifference); JMatrix q = orientationDifference * body2.invOrientation * body1.orientation; JVector axis; double x = q.M32 - q.M23; double y = q.M13 - q.M31; double z = q.M21 - q.M12; double r = JMath.Sqrt(x * x + y * y + z * z); double t = q.M11 + q.M22 + q.M33; double angle = (double)Math.Atan2(r, t - 1); axis = new JVector(x, y, z) * angle; if (r != 0.0f) { axis = axis * (1.0f / r); } bias = axis * biasFactor * (-1.0f / timestep); // Apply previous frame solution as initial guess for satisfying the constraint. if (!body1.IsStatic) { body1.angularVelocity += JVector.Transform(accumulatedImpulse, body1.invInertiaWorld); } if (!body2.IsStatic) { body2.angularVelocity += JVector.Transform(-1.0f * accumulatedImpulse, body2.invInertiaWorld); } }
protected override void rotate() { if (gameInput.keyboard[Key.Q]) { if (selectedMod != null && mConst == null) { JMatrix rotMatA = JMatrix.CreateRotationY(gameInput.move.X * cameraRotSpeed); JMatrix rotMatB = GenericMethods.FromOpenTKMatrix(Matrix4.CreateFromAxisAngle(-Parent.rightVec, gameInput.move.Y * cameraRotSpeed)); JMatrix rotMatFinal = JMatrix.Multiply(rotMatA, rotMatB); selectedBody.Orientation = JMatrix.Multiply(selectedBody.Orientation, rotMatFinal); } } else { base.rotate(); } }
public CompoundShape(List <TransformedShape> shapes) { this.shapes = shapes.ToArray(); var jshapes = new List <Jitter.Collision.Shapes.CompoundShape.TransformedShape>(); // Remove original reference model from scene graph foreach (var shape in shapes) { // TODO: Decompose - for now we only have rotations and translations so this should work JMatrix orientation = Util.ToJitter(shape.Transform); Vector3 p = shape.Transform.GetColumn3(3); JVector position = Util.ToJitter(p); Jitter.Collision.Shapes.Shape oldShape = shape.Shape.shape; Jitter.Collision.Shapes.CompoundShape compound = oldShape as Jitter.Collision.Shapes.CompoundShape; if (compound != null) { foreach (var part in compound.Shapes) { var newShape = new Jitter.Collision.Shapes.CompoundShape.TransformedShape( part.Shape, JMatrix.Multiply(orientation, part.Orientation), position + JVector.Transform(part.Position, orientation) ); jshapes.Add(newShape); } } else { var newShape = new Jitter.Collision.Shapes.CompoundShape.TransformedShape( oldShape, orientation, position ); jshapes.Add(newShape); } } compoundShape = new Jitter.Collision.Shapes.CompoundShape(jshapes); Matrix4 vertexOffset = Matrix4.CreateTranslation(compoundShape.Shift.X, compoundShape.Shift.Y, compoundShape.Shift.Z); Matrix4 modelOffset = Matrix4.CreateTranslation(-compoundShape.Shift.X, -compoundShape.Shift.Y, -compoundShape.Shift.Z); }
/// <summary> /// Add rigid-body to the draw-list to draw on the screen. /// Only rigid-bodies with "Tag" = "DrawMe" are drawn. /// </summary> /// <param name="rb">Rigidbody</param> private void AddBodyToDrawList(RigidBody rb) { if (rb.Tag is BodyTag && (BodyTag)rb.Tag == BodyTag.DontDrawMe) { return; } Collider c = rb as Collider; if (c?.GameObject.tag == ObjectTag.Ground) { return; } bool isCompoundShape = (rb.Shape is CompoundShape); if (!isCompoundShape) { AddShapeToDrawList(rb.Shape, rb.Orientation, rb.Position); } else { CompoundShape cShape = rb.Shape as CompoundShape; JMatrix orientation = rb.Orientation; JVector position = rb.Position; foreach (var ts in cShape.Shapes) { JVector pos = ts.Position; JMatrix ori = ts.Orientation; JVector.Transform(ref pos, ref orientation, out pos); JVector.Add(ref pos, ref position, out pos); JMatrix.Multiply(ref ori, ref orientation, out ori); AddShapeToDrawList(ts.Shape, ori, pos); } } }
private void AddBodyToDrawList(RigidBody rb) { if (rb.Tag is BodyTag && ((BodyTag)rb.Tag) == BodyTag.DontDrawMe) { return; } bool isCompoundShape = (rb.Shape is CompoundShape); if (!isCompoundShape) { //GraphicsDevice.BlendState = BlendState.Opaque; //GraphicsDevice.DepthStencilState = DepthStencilState.Default; AddShapeToDrawList(rb.Shape, rb.Orientation, rb.Position); } else { //GraphicsDevice.BlendState = BlendState.Opaque; //GraphicsDevice.DepthStencilState = DepthStencilState.None; CompoundShape cShape = rb.Shape as CompoundShape; JMatrix orientation = rb.Orientation; JVector position = rb.Position; foreach (var ts in cShape.Shapes) { JVector pos = ts.Position; JMatrix ori = ts.Orientation; JVector.Transform(ref pos, ref orientation, out pos); JVector.Add(ref pos, ref position, out pos); JMatrix.Multiply(ref ori, ref orientation, out ori); AddShapeToDrawList(ts.Shape, ori, pos); } } }
/// <summary> /// Calculates the inertia of the shape relative to the center of mass. /// </summary> /// <param name="shape"></param> /// <param name="centerOfMass"></param> /// <param name="inertia">Returns the inertia relative to the center of mass, not to the origin</param> /// <returns></returns> #region public static float CalculateMassInertia(Shape shape, out JVector centerOfMass, out JMatrix inertia) public static float CalculateMassInertia(Shape shape, out JVector centerOfMass, out JMatrix inertia) { float mass = 0.0f; centerOfMass = JVector.Zero; inertia = JMatrix.Zero; if (shape is Multishape) { throw new ArgumentException("Can't calculate inertia of multishapes.", "shape"); } // build a triangle hull around the shape List <JVector> hullTriangles = new List <JVector>(); shape.MakeHull(ref hullTriangles, 3); // create inertia of tetrahedron with vertices at // (0,0,0) (1,0,0) (0,1,0) (0,0,1) float a = 1.0f / 60.0f, b = 1.0f / 120.0f; JMatrix C = new JMatrix(a, b, b, b, a, b, b, b, a); for (int i = 0; i < hullTriangles.Count; i += 3) { JVector column0 = hullTriangles[i + 0]; JVector column1 = hullTriangles[i + 1]; JVector column2 = hullTriangles[i + 2]; JMatrix A = new JMatrix(column0.X, column1.X, column2.X, column0.Y, column1.Y, column2.Y, column0.Z, column1.Z, column2.Z); float detA = A.Determinant(); // now transform this canonical tetrahedron to the target tetrahedron // inertia by a linear transformation A JMatrix tetrahedronInertia = JMatrix.Multiply(A * C * JMatrix.Transpose(A), detA); JVector tetrahedronCOM = (1.0f / 4.0f) * (hullTriangles[i + 0] + hullTriangles[i + 1] + hullTriangles[i + 2]); float tetrahedronMass = (1.0f / 6.0f) * detA; inertia += tetrahedronInertia; centerOfMass += tetrahedronMass * tetrahedronCOM; mass += tetrahedronMass; } inertia = JMatrix.Multiply(JMatrix.Identity, inertia.Trace()) - inertia; centerOfMass = centerOfMass * (1.0f / mass); float x = centerOfMass.X; float y = centerOfMass.Y; float z = centerOfMass.Z; // now translate the inertia by the center of mass JMatrix t = new JMatrix( -mass * (y * y + z * z), mass * x * y, mass * x * z, mass * y * x, -mass * (z * z + x * x), mass * y * z, mass * z * x, mass * z * y, -mass * (x * x + y * y)); JMatrix.Add(ref inertia, ref t, out inertia); return(mass); }