/// <summary> /// Creates a new instance of the TransformedShape struct. /// </summary> /// <param name="shape">The shape.</param> /// <param name="orientation">The orientation this shape should have.</param> /// <param name="position">The position this shape should have.</param> public TransformedShape(Shape shape, JMatrix orientation, JVector position) { this.position = position; this.orientation = orientation; JMatrix.Transpose(ref orientation, out invOrientation); this.shape = shape; }
protected override void Update() { base.Update(); if (form.MouseClickedHappenend) { form.MouseClickedHappenend = false; var screenPosition = new JVector( -1f + 2* (form.MouseClickPosition.x / form.ClientSize.Width), 1, -1f + 2 * ((form.MouseClickPosition.y / form.ClientSize.Height))); var projectionMatrix = Common.ProjectionMatrix; JMatrix jMatrix = new JMatrix( projectionMatrix.M11, projectionMatrix.M12, projectionMatrix.M13, projectionMatrix.M21, projectionMatrix.M22, projectionMatrix.M23, projectionMatrix.M31, projectionMatrix.M32, projectionMatrix.M33); JMatrix invertedJMatrix; JMatrix.Invert(ref jMatrix, out invertedJMatrix); var rayDirection = JVector.Transform(screenPosition, invertedJMatrix); RigidBody body; JVector normal; float fraction; Entities.world3D.CollisionSystem.Raycast(JitterDatatypes.ToJVector(Common.CameraPosition), rayDirection, null, out body, out normal, out fraction); if (body != null && !body.IsStatic) body.ApplyImpulse(rayDirection * 200); } }
/// <summary> /// Constraints two bodies to always have the same relative /// orientation to each other. Combine the AngleConstraint with a PointOnLine /// Constraint to get a prismatic joint. /// </summary> public FixedAngle(RigidBody body1, RigidBody body2) : base(body1, body2) { initialOrientation1 = body1.orientation; initialOrientation2 = body2.orientation; //orientationDifference = body1.orientation * body2.invOrientation; //orientationDifference = JMatrix.Transpose(orientationDifference); }
internal RigidBodyState(RigidBody _body) { body = _body; savedPosition = body.Position; savedOrientation = body.Orientation; savedLinearVelocity = body.LinearVelocity; savedAngularVelocity = body.AngularVelocity; }
public static Matrix4 ToMatrix4(JMatrix orientation, JVector position) { return new Matrix4( orientation.M11, orientation.M12, orientation.M13, 0, orientation.M21, orientation.M22, orientation.M23, 0, orientation.M31, orientation.M32, orientation.M33, 0, position.X, position.Y, position.Z, 1); }
public static Matrix4 ToTritonMatrix(JMatrix matrix) { return new Matrix4( matrix.M11, matrix.M12, matrix.M13, 0.0f, matrix.M21, matrix.M22, matrix.M23, 0.0f, matrix.M31, matrix.M32, matrix.M33, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f); }
public static Matrix4 ToMatrix4(JMatrix m, JVector position) { return new Matrix4( m.M11, m.M12, m.M13, 0, m.M21, m.M22, m.M23, 0, m.M31, m.M32, m.M33, 0, position.X, position.Y, position.Z, 1); }
/// <summary> /// Calculates the bounding box of the sphere. /// </summary> /// <param name="orientation">The orientation of the shape.</param> /// <param name="box">The resulting axis aligned bounding box.</param> public override void GetBoundingBox(ref JMatrix orientation, out JBBox box) { box.Min.X = -radius; box.Min.Y = -radius; box.Min.Z = -radius; box.Max.X = radius; box.Max.Y = radius; box.Max.Z = radius; }
public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1, ref JMatrix orientation2, ref JVector position1, ref JVector position2, out JVector p1, out JVector p2, out JVector normal) { VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); simplexSolver.Reset(); p1 = p2 = JVector.Zero; JVector r = position1 - position2; JVector w, v; JVector supVertexA; JVector rn,vn; rn = JVector.Negate(r); SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA); JVector supVertexB; SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB); v = supVertexA - supVertexB; normal = JVector.Zero; int maxIter = 15; float distSq = v.LengthSquared(); float epsilon = 0.00001f; while ((distSq > epsilon) && (maxIter-- != 0)) { vn = JVector.Negate(v); SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA); SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB); w = supVertexA - supVertexB; if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB); if (simplexSolver.Closest(out v)) { distSq = v.LengthSquared(); normal = v; } else distSq = 0.0f; } simplexSolver.ComputePoints(out p1, out p2); if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon) normal.Normalize(); simplexSolverPool.GiveBack(simplexSolver); return true; }
static JMatrix() { Zero = new JMatrix(); Identity = new JMatrix(); Identity.M11 = 1.0f; Identity.M22 = 1.0f; Identity.M33 = 1.0f; InternalIdentity = Identity; }
public ConvexHullShape2( List <Jitter.LinearMath.JVector> vertices, Jitter.LinearMath.JMatrix inertia, float mass ) : base(vertices) { this.customInertia = inertia; this.customMass = mass; this.Inertia = inertia; this.Mass = mass; }
public static void Absolute(ref JMatrix matrix,out JMatrix result) { result.M11 = Math.Abs(matrix.M11); result.M12 = Math.Abs(matrix.M12); result.M13 = Math.Abs(matrix.M13); result.M21 = Math.Abs(matrix.M21); result.M22 = Math.Abs(matrix.M22); result.M23 = Math.Abs(matrix.M23); result.M31 = Math.Abs(matrix.M31); result.M32 = Math.Abs(matrix.M32); result.M33 = Math.Abs(matrix.M33); }
public static void Convert(JMatrix matrix, ref Matrix result) { result[0] = matrix.M11; result[1] = matrix.M12; result[2] = matrix.M13; result[4] = matrix.M21; result[5] = matrix.M22; result[6] = matrix.M23; result[8] = matrix.M31; result[9] = matrix.M32; result[10] = matrix.M33; }
/// <summary> /// Checks if given point is within a shape. /// </summary> /// <param name="support">The supportmap implementation representing the shape.</param> /// <param name="orientation">The orientation of the shape.</param> /// <param name="invOrientation">The inverse orientation of the shape.</param> /// <param name="position">The position of the shape.</param> /// <param name="point">The point to check.</param> /// <returns>Returns true if the point is within the shape, otherwise false.</returns> public static bool Pointcast(ISupportMappable support, ref JMatrix orientation,ref JVector position,ref JVector point) { JVector arbitraryPoint; SupportMapTransformed(support, ref orientation, ref position, ref point, out arbitraryPoint); JVector.Subtract(ref point, ref arbitraryPoint, out arbitraryPoint); JVector r; support.SupportCenter(out r); JVector.Transform(ref r, ref orientation, out r); JVector.Add(ref position, ref r, out r); JVector.Subtract(ref point, ref r, out r); JVector x = point; JVector w, p; float VdotR; JVector v; JVector.Subtract(ref x, ref arbitraryPoint, out v); float dist = v.LengthSquared(); float epsilon = 0.0001f; int maxIter = MaxIterations; VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); simplexSolver.Reset(); while ((dist > epsilon) && (maxIter-- != 0)) { SupportMapTransformed(support, ref orientation, ref position, ref v, out p); JVector.Subtract(ref x, ref p, out w); float VdotW = JVector.Dot(ref v, ref w); if (VdotW > 0.0f) { VdotR = JVector.Dot(ref v, ref r); if (VdotR >= -(JMath.Epsilon * JMath.Epsilon)) { simplexSolverPool.GiveBack(simplexSolver); return false; } else simplexSolver.Reset(); } if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, x, p); if (simplexSolver.Closest(out v)) dist = v.LengthSquared(); else dist = 0.0f; } simplexSolverPool.GiveBack(simplexSolver); return true; }
/// <summary> /// Gets the axis aligned bounding box of the orientated shape. This includes /// the whole shape. /// </summary> /// <param name="orientation">The orientation of the shape.</param> /// <param name="box">The axis aligned bounding box of the shape.</param> public override void GetBoundingBox(ref JMatrix orientation, out JBBox box) { JBBox helpBox = JBBox.LargeBox; int length = this.Prepare(ref helpBox); box = JBBox.SmallBox; for (int i = 0; i < length; i++) { this.SetCurrentShape(i); base.GetBoundingBox(ref orientation, out helpBox); JBBox.CreateMerged(ref box, ref helpBox, out box); } }
public static Matrix ToXNAMatrix(JMatrix matrix) { return new Matrix(matrix.M11, matrix.M12, matrix.M13, 0.0f, matrix.M21, matrix.M22, matrix.M23, 0.0f, matrix.M31, matrix.M32, matrix.M33, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f); }
private static void SupportMapTransformed(ISupportMappable support, ref JMatrix orientation, ref JVector position, ref JVector direction, out JVector result) { // THIS IS *THE* HIGH FREQUENCY CODE OF THE COLLLISION PART OF THE ENGINE result.X = ((direction.X * orientation.M11) + (direction.Y * orientation.M12)) + (direction.Z * orientation.M13); result.Y = ((direction.X * orientation.M21) + (direction.Y * orientation.M22)) + (direction.Z * orientation.M23); result.Z = ((direction.X * orientation.M31) + (direction.Y * orientation.M32)) + (direction.Z * orientation.M33); support.SupportMapping(ref result, out result); float x = ((result.X * orientation.M11) + (result.Y * orientation.M21)) + (result.Z * orientation.M31); float y = ((result.X * orientation.M12) + (result.Y * orientation.M22)) + (result.Z * orientation.M32); float z = ((result.X * orientation.M13) + (result.Y * orientation.M23)) + (result.Z * orientation.M33); result.X = position.X + x; result.Y = position.Y + y; result.Z = position.Z + z; }
public static DMatrix ToDoubleMatrix(JMatrix matrix) { DMatrix TM = new DMatrix(); TM.M11 = (float)matrix.M11; TM.M12 = (float)matrix.M12; TM.M13 = (float)matrix.M13; //TM.M14 = 0.0f; TM.M21 = (float)matrix.M21; TM.M22 = (float)matrix.M22; TM.M23 = (float)matrix.M23; //TM.M24 = 0.0f; TM.M31 = (float)matrix.M31; TM.M32 = (float)matrix.M32; TM.M33 = (float)matrix.M33; //TM.M34 = 0.0f; //TM.M41 = 0.0f; //TM.M42 = 0.0f; //TM.M43 = 0.0f; TM.M44 = 1.0f; return TM; }
private static void SupportMapTransformed(ISupportMappable support, ref JMatrix orientation, ref JVector position, ref JVector direction, out JVector result) { //JVector.Transform(ref direction, ref invOrientation, out result); //support.SupportMapping(ref result, out result); //JVector.Transform(ref result, ref orientation, out result); //JVector.Add(ref result, ref position, out result); result.X = ((direction.X * orientation.M11) + (direction.Y * orientation.M12)) + (direction.Z * orientation.M13); result.Y = ((direction.X * orientation.M21) + (direction.Y * orientation.M22)) + (direction.Z * orientation.M23); result.Z = ((direction.X * orientation.M31) + (direction.Y * orientation.M32)) + (direction.Z * orientation.M33); support.SupportMapping(ref result, out result); float x = ((result.X * orientation.M11) + (result.Y * orientation.M21)) + (result.Z * orientation.M31); float y = ((result.X * orientation.M12) + (result.Y * orientation.M22)) + (result.Z * orientation.M32); float z = ((result.X * orientation.M13) + (result.Y * orientation.M23)) + (result.Z * orientation.M33); result.X = position.X + x; result.Y = position.Y + y; result.Z = position.Z + z; }
/// <summary> /// Checks two shapes for collisions. /// </summary> /// <param name="support1">The SupportMappable implementation of the first shape to test.</param> /// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param> /// <param name="orientation1">The orientation of the first shape.</param> /// <param name="orientation2">The orientation of the second shape.</param> /// <param name="position1">The position of the first shape.</param> /// <param name="position2">The position of the second shape</param> /// <param name="point">The pointin world coordinates, where collision occur.</param> /// <param name="normal">The normal pointing from body2 to body1.</param> /// <param name="penetration">Estimated penetration depth of the collision.</param> /// <returns>Returns true if there is a collision, false otherwise.</returns> public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1, ref JMatrix orientation2, ref JVector position1, ref JVector position2, out JVector point, out JVector normal, out double penetration) { // Used variables JVector temp1, temp2; JVector v01, v02, v0; JVector v11, v12, v1; JVector v21, v22, v2; JVector v31, v32, v3; JVector v41, v42, v4; JVector mn; // Initialization of the output point = normal = JVector.Zero; penetration = 0.0f; //JVector right = JVector.Right; // Get the center of shape1 in world coordinates -> v01 support1.SupportCenter(out v01); JVector.Transform(ref v01, ref orientation1, out v01); JVector.Add(ref position1, ref v01, out v01); // Get the center of shape2 in world coordinates -> v02 support2.SupportCenter(out v02); JVector.Transform(ref v02, ref orientation2, out v02); JVector.Add(ref position2, ref v02, out v02); // v0 is the center of the minkowski difference JVector.Subtract(ref v02, ref v01, out v0); // Avoid case where centers overlap -- any direction is fine in this case if (v0.IsNearlyZero()) v0 = new JVector(0.00001f, 0, 0); // v1 = support in direction of origin mn = v0; JVector.Negate(ref v0, out normal); SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11); SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12); JVector.Subtract(ref v12, ref v11, out v1); if (JVector.Dot(ref v1, ref normal) <= 0.0f) return false; // v2 = support perpendicular to v1,v0 JVector.Cross(ref v1, ref v0, out normal); if (normal.IsNearlyZero()) { JVector.Subtract(ref v1, ref v0, out normal); normal.Normalize(); point = v11; JVector.Add(ref point, ref v12, out point); JVector.Multiply(ref point, 0.5f, out point); JVector.Subtract(ref v12, ref v11, out temp1); penetration = JVector.Dot(ref temp1, ref normal); //point = v11; //point2 = v12; return true; } JVector.Negate(ref normal, out mn); SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21); SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22); JVector.Subtract(ref v22, ref v21, out v2); if (JVector.Dot(ref v2, ref normal) <= 0.0f) return false; // Determine whether origin is on + or - side of plane (v1,v0,v2) JVector.Subtract(ref v1, ref v0, out temp1); JVector.Subtract(ref v2, ref v0, out temp2); JVector.Cross(ref temp1, ref temp2, out normal); double dist = JVector.Dot(ref normal, ref v0); // If the origin is on the - side of the plane, reverse the direction of the plane if (dist > 0.0f) { JVector.Swap(ref v1, ref v2); JVector.Swap(ref v11, ref v21); JVector.Swap(ref v12, ref v22); JVector.Negate(ref normal, out normal); } int phase2 = 0; int phase1 = 0; bool hit = false; // Phase One: Identify a portal while (true) { if (phase1 > MaximumIterations) return false; phase1++; // Obtain the support point in a direction perpendicular to the existing plane // Note: This point is guaranteed to lie off the plane JVector.Negate(ref normal, out mn); SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31); SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32); JVector.Subtract(ref v32, ref v31, out v3); if (JVector.Dot(ref v3, ref normal) <= 0.0f) { return false; } // If origin is outside (v1,v0,v3), then eliminate v2 and loop JVector.Cross(ref v1, ref v3, out temp1); if (JVector.Dot(ref temp1, ref v0) < 0.0f) { v2 = v3; v21 = v31; v22 = v32; JVector.Subtract(ref v1, ref v0, out temp1); JVector.Subtract(ref v3, ref v0, out temp2); JVector.Cross(ref temp1, ref temp2, out normal); continue; } // If origin is outside (v3,v0,v2), then eliminate v1 and loop JVector.Cross(ref v3, ref v2, out temp1); if (JVector.Dot(ref temp1, ref v0) < 0.0f) { v1 = v3; v11 = v31; v12 = v32; JVector.Subtract(ref v3, ref v0, out temp1); JVector.Subtract(ref v2, ref v0, out temp2); JVector.Cross(ref temp1, ref temp2, out normal); continue; } // Phase Two: Refine the portal // We are now inside of a wedge... while (true) { phase2++; // Compute normal of the wedge face JVector.Subtract(ref v2, ref v1, out temp1); JVector.Subtract(ref v3, ref v1, out temp2); JVector.Cross(ref temp1, ref temp2, out normal); // Can this happen??? Can it be handled more cleanly? if (normal.IsNearlyZero()) return true; normal.Normalize(); // Compute distance from origin to wedge face double d = JVector.Dot(ref normal, ref v1); // If the origin is inside the wedge, we have a hit if (d >= 0 && !hit) { // HIT!!! hit = true; } // Find the support point in the direction of the wedge face JVector.Negate(ref normal, out mn); SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41); SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42); JVector.Subtract(ref v42, ref v41, out v4); JVector.Subtract(ref v4, ref v3, out temp1); double delta = JVector.Dot(ref temp1, ref normal); penetration = JVector.Dot(ref v4, ref normal); // If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate if (delta <= CollideEpsilon || penetration <= 0.0f || phase2 > MaximumIterations) { if (hit) { JVector.Cross(ref v1, ref v2, out temp1); double b0 = JVector.Dot(ref temp1, ref v3); JVector.Cross(ref v3, ref v2, out temp1); double b1 = JVector.Dot(ref temp1, ref v0); JVector.Cross(ref v0, ref v1, out temp1); double b2 = JVector.Dot(ref temp1, ref v3); JVector.Cross(ref v2, ref v1, out temp1); double b3 = JVector.Dot(ref temp1, ref v0); double sum = b0 + b1 + b2 + b3; if (sum <= 0) { b0 = 0; JVector.Cross(ref v2, ref v3, out temp1); b1 = JVector.Dot(ref temp1, ref normal); JVector.Cross(ref v3, ref v1, out temp1); b2 = JVector.Dot(ref temp1, ref normal); JVector.Cross(ref v1, ref v2, out temp1); b3 = JVector.Dot(ref temp1, ref normal); sum = b1 + b2 + b3; } double inv = 1.0f / sum; JVector.Multiply(ref v01, b0, out point); JVector.Multiply(ref v11, b1, out temp1); JVector.Add(ref point, ref temp1, out point); JVector.Multiply(ref v21, b2, out temp1); JVector.Add(ref point, ref temp1, out point); JVector.Multiply(ref v31, b3, out temp1); JVector.Add(ref point, ref temp1, out point); JVector.Multiply(ref v02, b0, out temp2); JVector.Add(ref temp2, ref point, out point); JVector.Multiply(ref v12, b1, out temp1); JVector.Add(ref point, ref temp1, out point); JVector.Multiply(ref v22, b2, out temp1); JVector.Add(ref point, ref temp1, out point); JVector.Multiply(ref v32, b3, out temp1); JVector.Add(ref point, ref temp1, out point); JVector.Multiply(ref point, inv * 0.5f, out point); } // Compute the barycentric coordinates of the origin return hit; } //// Compute the tetrahedron dividing face (v4,v0,v1) //JVector.Cross(ref v4, ref v1, out temp1); //double d1 = JVector.Dot(ref temp1, ref v0); //// Compute the tetrahedron dividing face (v4,v0,v2) //JVector.Cross(ref v4, ref v2, out temp1); //double d2 = JVector.Dot(ref temp1, ref v0); // Compute the tetrahedron dividing face (v4,v0,v3) JVector.Cross(ref v4, ref v0, out temp1); double dot = JVector.Dot(ref temp1, ref v1); if (dot >= 0.0f) { dot = JVector.Dot(ref temp1, ref v2); if (dot >= 0.0f) { // Inside d1 & inside d2 ==> eliminate v1 v1 = v4; v11 = v41; v12 = v42; } else { // Inside d1 & outside d2 ==> eliminate v3 v3 = v4; v31 = v41; v32 = v42; } } else { dot = JVector.Dot(ref temp1, ref v3); if (dot >= 0.0f) { // Outside d1 & inside d3 ==> eliminate v2 v2 = v4; v21 = v41; v22 = v42; } else { // Outside d1 & outside d3 ==> eliminate v1 v1 = v4; v11 = v41; v12 = v42; } } } } }
/// <summary> /// Transforms a vector by the transposed of the given Matrix. /// </summary> /// <param name="position">The vector to transform.</param> /// <param name="matrix">The transform matrix.</param> /// <param name="result">The transformed vector.</param> public static void TransposedTransform(ref JVector position, ref JMatrix matrix, out JVector result) { float num0 = ((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13); float num1 = ((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23); float num2 = ((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33); result.X = num0; result.Y = num1; result.Z = num2; }
public static JVector Transform(JVector position, JMatrix matrix) { JVector result; JVector.Transform(ref position, ref matrix, out result); return result; }
/// <summary> /// Creates the transposed matrix. /// </summary> /// <param name="matrix">The matrix which should be transposed.</param> /// <param name="result">The transposed JMatrix.</param> public static void Transpose(ref JMatrix matrix, out JMatrix result) { result.M11 = matrix.M11; result.M12 = matrix.M21; result.M13 = matrix.M31; result.M21 = matrix.M12; result.M22 = matrix.M22; result.M23 = matrix.M32; result.M31 = matrix.M13; result.M32 = matrix.M23; result.M33 = matrix.M33; }
public static JMatrix Transpose(JMatrix matrix) { JMatrix result; JMatrix.Transpose(ref matrix, out result); return result; }
/// <summary> /// Multiply a matrix by a scalefactor. /// </summary> /// <param name="matrix1">The matrix.</param> /// <param name="scaleFactor">The scale factor.</param> /// <param name="result">A JMatrix multiplied by the scale factor.</param> public static void Multiply(ref JMatrix matrix1, double scaleFactor, out JMatrix result) { double num = scaleFactor; result.M11 = matrix1.M11 * num; result.M12 = matrix1.M12 * num; result.M13 = matrix1.M13 * num; result.M21 = matrix1.M21 * num; result.M22 = matrix1.M22 * num; result.M23 = matrix1.M23 * num; result.M31 = matrix1.M31 * num; result.M32 = matrix1.M32 * num; result.M33 = matrix1.M33 * num; }
/// <summary> /// Gets the axis aligned bounding box of the orientated shape. This includes /// the whole shape. /// </summary> /// <param name="orientation">The orientation of the shape.</param> /// <param name="box">The axis aligned bounding box of the shape.</param> public override void GetBoundingBox(ref JMatrix orientation, out JBBox box) { box = octree.rootNodeBox; #region Expand Spherical box.Min.X -= sphericalExpansion; box.Min.Y -= sphericalExpansion; box.Min.Z -= sphericalExpansion; box.Max.X += sphericalExpansion; box.Max.Y += sphericalExpansion; box.Max.Z += sphericalExpansion; #endregion box.Transform(ref orientation); }
/// <summary> /// Transforms a vector by the given matrix. /// </summary> /// <param name="position">The vector to transform.</param> /// <param name="matrix">The transform matrix.</param> /// <returns>The transformed vector.</returns> #region public static JVector Transform(JVector position, JMatrix matrix) public static JVector Transform(JVector position, JMatrix matrix) { Transform(ref position, ref matrix, out var result); return(result); }
/// <summary> /// Multiplies two matrices. /// </summary> /// <param name="value1">The first matrix.</param> /// <param name="value2">The second matrix.</param> /// <returns>The product of both values.</returns> #region public static JMatrix operator *(JMatrix value1,JMatrix value2) public static JMatrix operator *(JMatrix value1, JMatrix value2) { JMatrix result; JMatrix.Multiply(ref value1, ref value2, out result); return(result); }
/// <summary> /// Called once before iteration starts. /// </summary> /// <param name="timestep">The 5simulation timestep</param> public override void PrepareForIteration(float timestep) { effectiveMass = body1.invInertiaWorld; softnessOverDt = softness / timestep; effectiveMass.M11 += softnessOverDt; effectiveMass.M22 += softnessOverDt; effectiveMass.M33 += softnessOverDt; JMatrix.Inverse(ref effectiveMass, out effectiveMass); JMatrix q = JMatrix.Transpose(orientation) * body1.orientation; JVector axis; float x = q.M32 - q.M23; float y = q.M13 - q.M31; float z = q.M21 - q.M12; float r = JMath.Sqrt(x * x + y * y + z * z); float t = q.M11 + q.M22 + q.M33; float angle = (float)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); }
public static JMatrix Multiply(JMatrix matrix1, double scaleFactor) { JMatrix result; JMatrix.Multiply(ref matrix1, scaleFactor, out result); return result; }
/// <summary> /// Constraints two bodies to always have the same relative /// orientation to each other. Combine the AngleConstraint with a PointOnLine /// Constraint to get a prismatic joint. /// </summary> public FixedAngle(RigidBody body1) : base(body1, null) { orientation = body1.orientation; }
/// <summary> /// Adds two matrices. /// </summary> /// <param name="value1">The first matrix.</param> /// <param name="value2">The second matrix.</param> /// <returns>The sum of both values.</returns> #region public static JMatrix operator +(JMatrix value1, JMatrix value2) public static JMatrix operator +(JMatrix value1, JMatrix value2) { JMatrix result; JMatrix.Add(ref value1, ref value2, out result); return(result); }
public void Rotate(JMatrix orientation, JVector center) { for (int i = 0; i < points.Count; i++) { points[i].position = JVector.Transform(points[i].position - center, orientation); } }
/// <summary> /// Multiply two matrices. Notice: matrix multiplication is not commutative. /// </summary> /// <param name="matrix1">The first matrix.</param> /// <param name="matrix2">The second matrix.</param> /// <param name="result">The product of both matrices.</param> public static void Multiply(ref JMatrix matrix1, ref JMatrix matrix2, out JMatrix result) { double num0 = ((matrix1.M11 * matrix2.M11) + (matrix1.M12 * matrix2.M21)) + (matrix1.M13 * matrix2.M31); double num1 = ((matrix1.M11 * matrix2.M12) + (matrix1.M12 * matrix2.M22)) + (matrix1.M13 * matrix2.M32); double num2 = ((matrix1.M11 * matrix2.M13) + (matrix1.M12 * matrix2.M23)) + (matrix1.M13 * matrix2.M33); double num3 = ((matrix1.M21 * matrix2.M11) + (matrix1.M22 * matrix2.M21)) + (matrix1.M23 * matrix2.M31); double num4 = ((matrix1.M21 * matrix2.M12) + (matrix1.M22 * matrix2.M22)) + (matrix1.M23 * matrix2.M32); double num5 = ((matrix1.M21 * matrix2.M13) + (matrix1.M22 * matrix2.M23)) + (matrix1.M23 * matrix2.M33); double num6 = ((matrix1.M31 * matrix2.M11) + (matrix1.M32 * matrix2.M21)) + (matrix1.M33 * matrix2.M31); double num7 = ((matrix1.M31 * matrix2.M12) + (matrix1.M32 * matrix2.M22)) + (matrix1.M33 * matrix2.M32); double num8 = ((matrix1.M31 * matrix2.M13) + (matrix1.M32 * matrix2.M23)) + (matrix1.M33 * matrix2.M33); result.M11 = num0; result.M12 = num1; result.M13 = num2; result.M21 = num3; result.M22 = num4; result.M23 = num5; result.M31 = num6; result.M32 = num7; result.M33 = num8; }