/// <summary> /// Uses the supportMapping to calculate the bounding box. Should be overidden /// to make this faster. /// </summary> /// <param name="orientation">The orientation of the shape.</param> /// <param name="box">The resulting axis aligned bounding box.</param> public virtual void GetBoundingBox(ref TSMatrix orientation, out TSBBox box) { // I don't think that this can be done faster. // 6 is the minimum number of SupportMap calls. TSVector vec = TSVector.zero; vec.Set(orientation.M11, orientation.M21, orientation.M31); SupportMapping(ref vec, out vec); box.max.x = orientation.M11 * vec.x + orientation.M21 * vec.y + orientation.M31 * vec.z; vec.Set(orientation.M12, orientation.M22, orientation.M32); SupportMapping(ref vec, out vec); box.max.y = orientation.M12 * vec.x + orientation.M22 * vec.y + orientation.M32 * vec.z; vec.Set(orientation.M13, orientation.M23, orientation.M33); SupportMapping(ref vec, out vec); box.max.z = orientation.M13 * vec.x + orientation.M23 * vec.y + orientation.M33 * vec.z; vec.Set(-orientation.M11, -orientation.M21, -orientation.M31); SupportMapping(ref vec, out vec); box.min.x = orientation.M11 * vec.x + orientation.M21 * vec.y + orientation.M31 * vec.z; vec.Set(-orientation.M12, -orientation.M22, -orientation.M32); SupportMapping(ref vec, out vec); box.min.y = orientation.M12 * vec.x + orientation.M22 * vec.y + orientation.M32 * vec.z; vec.Set(-orientation.M13, -orientation.M23, -orientation.M33); SupportMapping(ref vec, out vec); box.min.z = orientation.M13 * vec.x + orientation.M23 * vec.y + orientation.M33 * vec.z; }
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2, out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration) { // Used variables TSVector center1, center2; // Initialization of the output point = point1 = point2 = normal = TSVector.zero; penetration = FP.Zero; BoxShape box1 = this.Shape1 as BoxShape; BoxShape box2 = this.Shape2 as BoxShape; // Get the center of box1 in world coordinates -> center1 box1.SupportCenter(out center1); TSVector.Transform(ref center1, ref orientation1, out center1); TSVector.Add(ref position1, ref center1, out center1); // Get the center of box2 in world coordinates -> center2 box2.SupportCenter(out center2); TSVector.Transform(ref center2, ref orientation2, out center2); TSVector.Add(ref position2, ref center2, out center2); // TODO: box-box collision test return(true); }
/// <summary> /// Creates a quaternion from a matrix. /// </summary> /// <param name="matrix">A matrix representing an orientation.</param> /// <returns>JQuaternion representing an orientation.</returns> #region public static JQuaternion CreateFromMatrix(JMatrix matrix) public static TSQuaternion CreateFromMatrix(TSMatrix matrix) { TSQuaternion result; TSQuaternion.CreateFromMatrix(ref matrix, out result); return(result); }
/// <summary> /// Gets the determinant of the matrix. /// </summary> /// <returns>The determinant of the matrix.</returns> #region public FP Determinant() //public FP Determinant() //{ // return M11 * M22 * M33 -M11 * M23 * M32 -M12 * M21 * M33 +M12 * M23 * M31 + M13 * M21 * M32 - M13 * M22 * M31; //} #endregion /// <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> /// <returns>The product of both matrices.</returns> #region public static JMatrix Multiply(JMatrix matrix1, JMatrix matrix2) public static TSMatrix Multiply(TSMatrix matrix1, TSMatrix matrix2) { TSMatrix result; TSMatrix.Multiply(ref matrix1, ref matrix2, out result); return(result); }
/// <summary> /// By calling this method the shape inertia and mass is used. /// </summary> public void SetMassProperties() { this.inertia = Shape.inertia; TSMatrix.Inverse(ref inertia, out invInertia); this.inverseMass = FP.One / Shape.mass; useShapeMassProperties = true; }
/// <summary> /// Recalculates the axis aligned bounding box and the inertia /// values in world space. /// </summary> public virtual void Update() { if (isParticle) { this.inertia = TSMatrix.Zero; this.invInertia = this.invInertiaWorld = TSMatrix.Zero; this.invOrientation = this.orientation = TSMatrix.Identity; this.boundingBox = shape.boundingBox; TSVector.Add(ref boundingBox.min, ref this.position, out boundingBox.min); TSVector.Add(ref boundingBox.max, ref this.position, out boundingBox.max); angularVelocity.MakeZero(); } else { // Given: Orientation, Inertia TSMatrix.Transpose(ref orientation, out invOrientation); this.Shape.GetBoundingBox(ref orientation, out boundingBox); TSVector.Add(ref boundingBox.min, ref this.position, out boundingBox.min); TSVector.Add(ref boundingBox.max, ref this.position, out boundingBox.max); if (!isStatic) { TSMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld); TSMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld); } } }
/// <summary> /// Matrices are added. /// </summary> /// <param name="matrix1">The first matrix.</param> /// <param name="matrix2">The second matrix.</param> /// <returns>The sum of both matrices.</returns> #region public static JMatrix Add(JMatrix matrix1, JMatrix matrix2) public static TSMatrix Add(TSMatrix matrix1, TSMatrix matrix2) { TSMatrix result; TSMatrix.Add(ref matrix1, ref matrix2, out result); return(result); }
public void Rotate(TSMatrix orientation, TSVector center) { for (int i = 0; i < points.Count; i++) { points[i].position = TSVector.Transform(points[i].position - center, orientation); } }
public void Clone(Shape sh) { this.inertia = sh.inertia; this.mass = sh.mass; this.boundingBox = sh.boundingBox; this.geomCen = sh.geomCen; if (sh is BoxShape) { CloneBox((BoxShape)sh); } else if (sh is SphereShape) { CloneSphere((SphereShape)sh); } else if (sh is ConeShape) { CloneCone((ConeShape)sh); } else if (sh is CylinderShape) { CloneCylinder((CylinderShape)sh); } else if (sh is CapsuleShape) { CloneCapsule((CapsuleShape)sh); } }
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2, out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration) { // Used variables TSVector center1, center2; // Initialization of the output point = point1 = point2 = normal = TSVector.zero; penetration = FP.Zero; BoxShape box = this.Shape1 as BoxShape; SphereShape sphere = this.Shape2 as SphereShape; // Get the center of box in world coordinates -> center1 box.SupportCenter(out center1); TSVector.Transform(ref center1, ref orientation1, out center1); TSVector.Add(ref position1, ref center1, out center1); // Get the center of sphere in world coordinates -> center2 sphere.SupportCenter(out center2); TSVector.Transform(ref center2, ref orientation2, out center2); TSVector.Add(ref position2, ref center2, out center2); FP dist = GetSphereDistance(ref box, ref center1, ref orientation1, ref center2, sphere.radius, ref point1, ref point2, ref normal); if (dist < TSMath.Epsilon) { penetration = -dist; return(true); } return(false); }
/// <summary> /// Creates a 4x4 matrix from a 3x3 matrix. /// </summary> /// <param name="a">3x3 matrix.</param> /// <returns>Created 4x4 matrix.</returns> public static TSMatrix4x4 ToMatrix4X4(TSMatrix a) { TSMatrix4x4 b; b.M11 = a.M11; b.M12 = a.M12; b.M13 = a.M13; b.M21 = a.M21; b.M22 = a.M22; b.M23 = a.M23; b.M31 = a.M31; b.M32 = a.M32; b.M33 = a.M33; b.M44 = FP.One; b.M14 = FP.Zero; b.M24 = FP.Zero; b.M34 = FP.Zero; b.M41 = FP.Zero; b.M42 = FP.Zero; b.M43 = FP.Zero; return(b); }
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2, out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration) { // Used variables TSVector center1, center2; // Initialization of the output point = point1 = point2 = normal = TSVector.zero; penetration = FP.Zero; TriangleMeshShape triangle = this.Shape1 as TriangleMeshShape; SphereShape sphere = this.Shape2 as SphereShape; // Get the center of sphere in world coordinates -> center1 triangle.SupportCenter(out center1); TSVector.Transform(ref center1, ref orientation1, out center1); TSVector.Add(ref position1, ref center1, out center1); // Get the center of triangle in world coordinates -> center2 sphere.SupportCenter(out center2); TSVector.Transform(ref center2, ref orientation2, out center2); TSVector.Add(ref position2, ref center2, out center2); TSVector[] vertices = triangle.Vertices; TSVector.Transform(ref vertices[0], ref orientation1, out vertices[0]); TSVector.Add(ref position1, ref vertices[0], out vertices[0]); TSVector.Transform(ref vertices[1], ref orientation1, out vertices[1]); TSVector.Add(ref position1, ref vertices[1], out vertices[1]); TSVector.Transform(ref vertices[2], ref orientation1, out vertices[2]); TSVector.Add(ref position1, ref vertices[2], out vertices[2]); return(Collide(center2, sphere.radius, ref vertices, ref point, ref point1, ref point2, ref normal, ref penetration)); }
public override void CalculateMassInertia() { base.inertia = TSMatrix.Zero; base.mass = FP.Zero; for (int i = 0; i < Shapes.Length; i++) { TSMatrix currentInertia = Shapes[i].InverseOrientation * Shapes[i].Shape.Inertia * Shapes[i].Orientation; TSVector p = Shapes[i].Position * -FP.One; FP m = Shapes[i].Shape.Mass; currentInertia.M11 += m * (p.y * p.y + p.z * p.z); currentInertia.M22 += m * (p.x * p.x + p.z * p.z); currentInertia.M33 += m * (p.x * p.x + p.y * p.y); currentInertia.M12 += -p.x * p.y * m; currentInertia.M21 += -p.x * p.y * m; currentInertia.M31 += -p.x * p.z * m; currentInertia.M13 += -p.x * p.z * m; currentInertia.M32 += -p.y * p.z * m; currentInertia.M23 += -p.y * p.z * m; base.inertia += currentInertia; base.mass += m; } }
/// <summary> /// Calculates the inverse of a give matrix. /// </summary> /// <param name="matrix">The matrix to invert.</param> /// <returns>The inverted JMatrix.</returns> #region public static JMatrix Inverse(JMatrix matrix) public static TSMatrix Inverse(TSMatrix matrix) { TSMatrix result; TSMatrix.Inverse(ref matrix, out result); return(result); }
public static void Invert(ref TSMatrix matrix, out TSMatrix result) { FP determinantInverse = 1 / matrix.Determinant(); FP m11 = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * determinantInverse; FP m12 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * determinantInverse; FP m13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * determinantInverse; FP m21 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * determinantInverse; FP m22 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * determinantInverse; FP m23 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * determinantInverse; FP m31 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * determinantInverse; FP m32 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * determinantInverse; FP m33 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * determinantInverse; result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; result.M31 = m31; result.M32 = m32; result.M33 = m33; }
/// <summary> /// Initializes a new instance of the RigidBody class. /// </summary> /// <param name="shape">The shape of the body.</param> /// <param name="isParticle">If set to true the body doesn't rotate. /// Also contacts are only solved for the linear motion part.</param> public RigidBody(Shape shape, BodyMaterial material, bool isParticle) { readOnlyArbiters = new ReadOnlyHashset <Arbiter>(arbiters); readOnlyConstraints = new ReadOnlyHashset <Constraint>(constraints); instanceCount++; instance = instanceCount; hashCode = CalculateHash(instance); this.Shape = shape; orientation = TSMatrix.Identity; if (!isParticle) { updatedHandler = new ShapeUpdatedHandler(ShapeUpdated); this.Shape.ShapeUpdated += updatedHandler; SetMassProperties(); } else { this.inertia = TSMatrix.Zero; this.invInertia = this.invInertiaWorld = TSMatrix.Zero; this.invOrientation = this.orientation = TSMatrix.Identity; inverseMass = FP.One; } AllowDeactivation = true; EnableSpeculativeContacts = false; this.isParticle = isParticle; Update(); }
/// <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 TSVector Transform(TSVector position, TSMatrix matrix) { TSVector result; TSVector.Transform(ref position, ref matrix, out result); return(result); }
public override void CalculateMassInertia() { base.inertia = TSMatrix.Zero; base.mass = FP.Zero; for (int i = 0; i < Shapes.Length; i++) { TSMatrix currentInertia = Shapes[i].InverseOrientation * Shapes[i].Shape.Inertia * Shapes[i].Orientation; TSVector p = Shapes[i].Position * -FP.One; FP m = Shapes[i].Shape.Mass; FP xx = p.x * p.x; FP yy = p.y * p.y; FP zz = p.z * p.z; FP xym = p.x * p.y * m; FP xzm = p.x * p.z * m; FP yzm = p.y * p.z * m; currentInertia.M11 += m * (yy + zz); currentInertia.M22 += m * (xx + zz); currentInertia.M33 += m * (xx + yy); currentInertia.M12 += -xym; currentInertia.M21 += -xym; currentInertia.M31 += -xzm; currentInertia.M13 += -xzm; currentInertia.M32 += -yzm; currentInertia.M23 += -yzm; base.inertia += currentInertia; base.mass += m; } }
/// <summary> /// Subtracts two matrices. /// </summary> /// <param name="value1">The first matrix.</param> /// <param name="value2">The second matrix.</param> /// <returns>The difference of both values.</returns> #region public static JMatrix operator -(JMatrix value1, JMatrix value2) public static TSMatrix operator -(TSMatrix value1, TSMatrix value2) { TSMatrix result; TSMatrix.Multiply(ref value2, -FP.One, out value2); TSMatrix.Add(ref value1, ref value2, out result); return(result); }
/// <summary> /// Creates the transposed matrix. /// </summary> /// <param name="matrix">The matrix which should be transposed.</param> /// <returns>The transposed JMatrix.</returns> #region public static JMatrix Transpose(JMatrix matrix) public static TSMatrix Transpose(TSMatrix matrix) { TSMatrix result; TSMatrix.Transpose(ref matrix, out result); return(result); }
public static TSMatrix CreateFromQuaternion(TSQuaternion quaternion) { TSMatrix result; TSMatrix.CreateFromQuaternion(ref quaternion, 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> /// <returns>A JMatrix multiplied by the scale factor.</returns> #region public static JMatrix Multiply(JMatrix matrix1, FP scaleFactor) public static TSMatrix Multiply(TSMatrix matrix1, FP scaleFactor) { TSMatrix result; TSMatrix.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, RigidBody body2) : base(body1, body2) { initialOrientation1 = body1.orientation; initialOrientation2 = body2.orientation; //orientationDifference = body1.orientation * body2.invOrientation; //orientationDifference = JMatrix.Transpose(orientationDifference); }
/// <summary> /// By calling this method the shape inertia and mass is used. /// </summary> public void SetMassProperties() { TSLinearDrag = TrueSyncManager.Config.linearDrag; TSAngularDrag = TrueSyncManager.Config.angularDrag; this.inertia = Shape.inertia; TSMatrix.Inverse(ref inertia, out invInertia); this.inverseMass = FP.One / Shape.mass; useShapeMassProperties = true; }
/// <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, TSMatrix orientation, TSVector position) { this.position = position; this.orientation = orientation; TSMatrix.Transpose(ref orientation, out invOrientation); this.shape = shape; this.boundingBox = new TSBBox(); UpdateBoundingBox(); }
/// <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 TSMatrix orientation, out TSBBox 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 RigidBodyData() { inertia = TSMatrix.InternalIdentity; invInertia = TSMatrix.InternalIdentity; invInertiaWorld = TSMatrix.InternalIdentity; orientation = TSMatrix.InternalIdentity; invOrientation = TSMatrix.InternalIdentity; position = TSVector.zero; linearVelocity = TSVector.zero; angularVelocity = TSVector.zero; staticFriction = FP.Zero; restitution = FP.Zero; boundingBox = new TSBBox(); inactiveTime = FP.Zero; isActive = true; isStatic = false; isKinematic = false; affectedByGravity = true; isColliderOnly = false; inverseMass = FP.Zero; force = TSVector.zero; torque = TSVector.zero; hashCode = 0; internalIndex = 0; marker = 0; disabled = false; _freezePosition = TSVector.zero; _freezeRotation = TSMatrix.InternalIdentity; _freezeRotationQuat = TSQuaternion.identity; // Previous state of gravity before switch Kinematic to true prevKinematicGravity = false; linearDrag = FP.Zero; angularDrag = FP.Zero; isParticle = false; shape = new ShapeData(); damping = DampingType.Angular | DampingType.Linear; }
public void Clone(RigidBody rb) { this.marker = rb.marker; this.affectedByGravity = rb.affectedByGravity; this.boundingBox = rb.boundingBox; this.internalIndex = rb.internalIndex; this.inverseMass = rb.inverseMass; this.isColliderOnly = rb.isColliderOnly; this.isStatic = rb.isStatic; this.isKinematic = rb.isKinematic; this.sweptDirection = rb.sweptDirection; this.position = rb.Position; this.orientation = rb.Orientation; this.linearVelocity = rb.LinearVelocity; CBFrame.Utils.Logger.Debug("line401 this.linearVelocity:" + this.linearVelocity); this.angularVelocity = rb.AngularVelocity; this.inertia = rb.Inertia; this.invInertia = rb.InverseInertia; this.invInertiaWorld = rb.InverseInertiaWorld; this.invOrientation = rb.invOrientation; this.force = rb.Force; this.torque = rb.Torque; this.shapeClone = poolGenericShapeClone.GetNew(); this.shapeClone.Clone(rb.Shape); this.connections.Clear(); for (index = 0, length = rb.connections.Count; index < length; index++) { this.connections.Add(rb.connections[index]); } this.constraints.Clear(); for (index = 0, length = rb.constraints.Count; index < length; index++) { this.constraints.Add(rb.constraints[index]); } this.isActive = rb.IsActive; this.inactiveTime = rb.inactiveTime; this.marker = rb.marker; this.disabled = rb.disabled; this.freezeConstraint = rb._freezeConstraints; this._freezePosition = rb._freezePosition; this._freezeRotation = rb._freezeRotation; this._freezeRotationQuat = rb._freezeRotationQuat; this.prevKinematicGravity = rb.prevKinematicGravity; this.linearDrag = rb.linearDrag; this.angularDrag = rb.angularDrag; this.staticFriction = rb.staticFriction; this.restitution = rb.restitution; }
private void IntegrateCallback(object obj) { RigidBody body = obj as RigidBody; TSVector temp; TSVector.Multiply(ref body.linearVelocity, timestep, out temp); TSVector.Add(ref temp, ref body.position, out body.position); if (!(body.isParticle)) { //exponential map TSVector axis; FP angle = body.angularVelocity.magnitude; FP halfTimeStep = FP.Half * timestep; FP halfTimeStepAngle = halfTimeStep * angle; if (angle < FP.EN3) { // use Taylor's expansions of sync function // axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle); //TSVector.Multiply(ref body.angularVelocity, (halfTimeStep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis); TSVector.Multiply(ref body.angularVelocity, halfTimeStep, out axis); } else { // sync(fAngle) = sin(c*fAngle)/t TSVector.Multiply(ref body.angularVelocity, (FP.Sin(halfTimeStepAngle) / angle), out axis); } TSQuaternion dorn = new TSQuaternion(axis.x, axis.y, axis.z, FP.Cos(halfTimeStepAngle)); TSQuaternion ornA; TSQuaternion.CreateFromMatrix(ref body.orientation, out ornA); TSQuaternion.Multiply(ref dorn, ref ornA, out dorn); dorn.Normalize(); TSMatrix.CreateFromQuaternion(ref dorn, out body.orientation); } body.linearVelocity /= (1 + timestep * body.linearDrag); body.angularVelocity /= (1 + timestep * body.angularDrag); /*if ((body.Damping & RigidBody.DampingType.Linear) != 0) * TSVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity); * * if ((body.Damping & RigidBody.DampingType.Angular) != 0) * TSVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);*/ body.Update(); if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts) { body.SweptExpandBoundingBox(timestep); } }
/// <summary> /// Gets the axis aligned bounding box of the orientated 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 TSMatrix orientation, out TSBBox box) { TSMatrix abs; TSMath.Absolute(ref orientation, out abs); TSVector temp; TSVector.Transform(ref halfSize, ref abs, out temp); box.max = temp; TSVector.Negate(ref temp, out box.min); }