protected internal override void GetContactInformation(int index, out ContactInformation info) { info.Contact = MeshManifold.contacts.Elements[index]; //Find the contact's normal and friction forces. info.FrictionImpulse = 0; info.NormalImpulse = 0; for (int i = 0; i < contactConstraint.frictionConstraints.Count; i++) { if (contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.contact == info.Contact) { info.FrictionImpulse = contactConstraint.frictionConstraints.Elements[i].accumulatedImpulse; info.NormalImpulse = contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.accumulatedImpulse; break; } } //Compute relative velocity if (convex.entity != null) { System.Numerics.Vector3 velocity; Vector3Ex.Subtract(ref info.Contact.Position, ref convex.entity.position, out velocity); Vector3Ex.Cross(ref convex.entity.angularVelocity, ref velocity, out velocity); Vector3Ex.Add(ref velocity, ref convex.entity.linearVelocity, out info.RelativeVelocity); } else { info.RelativeVelocity = new System.Numerics.Vector3(); } info.Pair = this; }
protected internal override void UpdateJacobiansAndVelocityBias() { linearJacobian = new Matrix3x3(); System.Numerics.Vector3 boneAxis; QuaternionEx.Transform(ref BoneLocalFreeAxis, ref TargetBone.Orientation, out boneAxis); angularJacobian = new Matrix3x3 { M11 = constrainedAxis1.X, M12 = constrainedAxis1.Y, M13 = constrainedAxis1.Z, M21 = constrainedAxis2.X, M22 = constrainedAxis2.Y, M23 = constrainedAxis2.Z }; System.Numerics.Vector3 error; Vector3Ex.Cross(ref boneAxis, ref freeAxis, out error); System.Numerics.Vector2 constraintSpaceError; Vector3Ex.Dot(ref error, ref constrainedAxis1, out constraintSpaceError.X); Vector3Ex.Dot(ref error, ref constrainedAxis2, out constraintSpaceError.Y); velocityBias.X = errorCorrectionFactor * constraintSpaceError.X; velocityBias.Y = errorCorrectionFactor * constraintSpaceError.Y; }
/// <summary> /// Sets up the joint transforms by automatically creating perpendicular vectors to complete the bases. /// </summary> /// <param name="worldTwistAxisA">Twist axis in world space to attach to entity A.</param> /// <param name="worldTwistAxisB">Twist axis in world space to attach to entity B.</param> public void SetupJointTransforms(System.Numerics.Vector3 worldTwistAxisA, System.Numerics.Vector3 worldTwistAxisB) { worldTwistAxisA.Normalize(); worldTwistAxisB.Normalize(); System.Numerics.Vector3 worldXAxis; Vector3Ex.Cross(ref worldTwistAxisA, ref Toolbox.UpVector, out worldXAxis); float length = worldXAxis.LengthSquared(); if (length < Toolbox.Epsilon) { Vector3Ex.Cross(ref worldTwistAxisA, ref Toolbox.RightVector, out worldXAxis); } worldXAxis.Normalize(); //Complete A's basis. System.Numerics.Vector3 worldYAxis; Vector3Ex.Cross(ref worldTwistAxisA, ref worldXAxis, out worldYAxis); basisA.rotationMatrix = connectionA.orientationMatrix; basisA.SetWorldAxes(worldTwistAxisA, worldXAxis, worldYAxis); //Rotate the axis to B since it could be arbitrarily rotated. System.Numerics.Quaternion rotation; QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref worldTwistAxisA, ref worldTwistAxisB, out rotation); QuaternionEx.Transform(ref worldXAxis, ref rotation, out worldXAxis); basisB.rotationMatrix = connectionB.orientationMatrix; basisB.SetWorldAxes(worldTwistAxisB, worldXAxis); }
public void CalculateFrustum(int width, int height, Vector3 origin) { frustumForRays.Planes = new Plane[4]; Vector3 cornerRay0 = rayArray[0].directionNormal; Vector3 cornerRay1 = rayArray[width - 1].directionNormal; Vector3 cornerRay2 = rayArray[(height - 1) * width].directionNormal; Vector3 cornerRay3 = rayArray[(height - 1) * width + (width - 1)].directionNormal; { Vector3 normal = Vector3Ex.Cross(cornerRay0, cornerRay1).GetNormal(); frustumForRays.Planes[0] = new Plane(normal, Vector3Ex.Dot(normal, origin)); } { Vector3 normal = Vector3Ex.Cross(cornerRay1, cornerRay2).GetNormal(); frustumForRays.Planes[1] = new Plane(normal, Vector3Ex.Dot(normal, origin)); } { Vector3 normal = Vector3Ex.Cross(cornerRay2, cornerRay3).GetNormal(); frustumForRays.Planes[2] = new Plane(normal, Vector3Ex.Dot(normal, origin)); } { Vector3 normal = Vector3Ex.Cross(cornerRay3, cornerRay0).GetNormal(); frustumForRays.Planes[3] = new Plane(normal, Vector3Ex.Dot(normal, origin)); } }
/// <summary> /// Automatically computes the measurement axes for the current local axes. /// The current relative state of the entities will be considered 0 twist angle. /// </summary> public void ComputeMeasurementAxes() { System.Numerics.Vector3 axisA, axisB; QuaternionEx.Transform(ref LocalAxisA, ref ConnectionA.Orientation, out axisA); QuaternionEx.Transform(ref LocalAxisB, ref ConnectionB.Orientation, out axisB); //Pick an axis perpendicular to axisA to use as the measurement axis. System.Numerics.Vector3 worldMeasurementAxisA; Vector3Ex.Cross(ref Toolbox.UpVector, ref axisA, out worldMeasurementAxisA); float lengthSquared = worldMeasurementAxisA.LengthSquared(); if (lengthSquared > Toolbox.Epsilon) { Vector3Ex.Divide(ref worldMeasurementAxisA, (float)Math.Sqrt(lengthSquared), out worldMeasurementAxisA); } else { //Oops! It was parallel to the up vector. Just try again with the right vector. Vector3Ex.Cross(ref Toolbox.RightVector, ref axisA, out worldMeasurementAxisA); worldMeasurementAxisA.Normalize(); } //Attach the measurement axis to entity B. //'Push' A's axis onto B by taking into account the swing transform. System.Numerics.Quaternion alignmentRotation; QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref axisA, ref axisB, out alignmentRotation); System.Numerics.Vector3 worldMeasurementAxisB; QuaternionEx.Transform(ref worldMeasurementAxisA, ref alignmentRotation, out worldMeasurementAxisB); //Plop them on! MeasurementAxisA = worldMeasurementAxisA; MeasurementAxisB = worldMeasurementAxisB; }
/// <summary> /// Updates the movement basis of the horizontal motion constraint. /// Should be updated automatically by the character on each time step; other code should not need to call this. /// </summary> /// <param name="forward">Forward facing direction of the character.</param> public void UpdateMovementBasis(ref System.Numerics.Vector3 forward) { System.Numerics.Vector3 down = characterBody.orientationMatrix.Down; System.Numerics.Vector3 strafeDirection; System.Numerics.Vector3 horizontalForwardDirection = forward - down * Vector3Ex.Dot(down, forward); float forwardLengthSquared = horizontalForwardDirection.LengthSquared(); if (forwardLengthSquared < Toolbox.Epsilon) { //Use an arbitrary direction to complete the basis. horizontalForwardDirection = characterBody.orientationMatrix.Forward; strafeDirection = characterBody.orientationMatrix.Right; } else { Vector3Ex.Divide(ref horizontalForwardDirection, (float)Math.Sqrt(forwardLengthSquared), out horizontalForwardDirection); Vector3Ex.Cross(ref down, ref horizontalForwardDirection, out strafeDirection); //Don't need to normalize the strafe direction; it's the cross product of two normalized perpendicular vectors. } Vector3Ex.Multiply(ref horizontalForwardDirection, movementDirection.Y, out movementDirection3d); System.Numerics.Vector3 strafeComponent; Vector3Ex.Multiply(ref strafeDirection, movementDirection.X, out strafeComponent); Vector3Ex.Add(ref strafeComponent, ref movementDirection3d, out movementDirection3d); }
private void Initialize() { //Compute a vector which is perpendicular to the axis. It'll be added in local space to both connections. System.Numerics.Vector3 yAxis; Vector3Ex.Cross(ref worldAxisA, ref Toolbox.UpVector, out yAxis); float length = yAxis.LengthSquared(); if (length < Toolbox.Epsilon) { Vector3Ex.Cross(ref worldAxisA, ref Toolbox.RightVector, out yAxis); } yAxis.Normalize(); //Put the axis into the local space of A. System.Numerics.Quaternion conjugate; QuaternionEx.Conjugate(ref connectionA.orientation, out conjugate); QuaternionEx.Transform(ref yAxis, ref conjugate, out aLocalAxisY); //Complete A's basis. Vector3Ex.Cross(ref localAxisA, ref aLocalAxisY, out aLocalAxisZ); //Rotate the axis to B since it could be arbitrarily rotated. System.Numerics.Quaternion rotation; QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref worldAxisA, ref worldAxisB, out rotation); QuaternionEx.Transform(ref yAxis, ref rotation, out yAxis); //Put it into local space. QuaternionEx.Conjugate(ref connectionB.orientation, out conjugate); QuaternionEx.Transform(ref yAxis, ref conjugate, out bLocalAxisY); }
/// <summary> /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// </summary> public override void ExclusiveUpdate() { //Warm starting //Constraint.applyImpulse(myConnectionA, myConnectionB, ref rA, ref rB, ref accumulatedImpulse); #if !WINDOWS System.Numerics.Vector3 linear = new System.Numerics.Vector3(); #else System.Numerics.Vector3 linear; #endif if (connectionA.isDynamic) { linear.X = -accumulatedImpulse.X; linear.Y = -accumulatedImpulse.Y; linear.Z = -accumulatedImpulse.Z; connectionA.ApplyLinearImpulse(ref linear); System.Numerics.Vector3 taImpulse; Vector3Ex.Cross(ref worldOffsetA, ref linear, out taImpulse); connectionA.ApplyAngularImpulse(ref taImpulse); } if (connectionB.isDynamic) { connectionB.ApplyLinearImpulse(ref accumulatedImpulse); System.Numerics.Vector3 tbImpulse; Vector3Ex.Cross(ref worldOffsetB, ref accumulatedImpulse, out tbImpulse); connectionB.ApplyAngularImpulse(ref tbImpulse); } }
public override (double u, double v) GetUv(IntersectInfo info) { Vector3 vn = new Vector3(0, 1, 0).GetNormal(); // north pole / up Vector3 ve = new Vector3(0, 0, 1).GetNormal(); // equator / sphere orientation Vector3 vp = (info.HitPosition - position).GetNormal(); //points from center of sphere to intersection double phi = Math.Acos(-Vector3Ex.Dot(vp, vn)); double v = (phi * 2 / Math.PI) - 1; double sinphi = Vector3Ex.Dot(ve, vp) / Math.Sin(phi); sinphi = sinphi < -1 ? -1 : sinphi > 1 ? 1 : sinphi; double theta = Math.Acos(sinphi) * 2 / Math.PI; double u; if (Vector3Ex.Dot(Vector3Ex.Cross(vn, ve), vp) > 0) { u = theta; } else { u = 1 - theta; } return(u, v); }
/// <summary> /// Updates the position of the detector before each step. /// </summary> protected internal override void UpdateDetectorPosition() { #if !WINDOWS System.Numerics.Vector3 newPosition = new System.Numerics.Vector3(); #else System.Numerics.Vector3 newPosition; #endif newPosition.X = wheel.suspension.worldAttachmentPoint.X + wheel.suspension.worldDirection.X * wheel.suspension.restLength * .5f; newPosition.Y = wheel.suspension.worldAttachmentPoint.Y + wheel.suspension.worldDirection.Y * wheel.suspension.restLength * .5f; newPosition.Z = wheel.suspension.worldAttachmentPoint.Z + wheel.suspension.worldDirection.Z * wheel.suspension.restLength * .5f; detector.Position = newPosition; if (IncludeSteeringTransformInCast) { System.Numerics.Quaternion localSteeringTransform; QuaternionEx.CreateFromAxisAngle(ref wheel.suspension.localDirection, steeringAngle, out localSteeringTransform); detector.Orientation = QuaternionEx.Concatenate(localSteeringTransform, wheel.Vehicle.Body.orientation); } else { detector.Orientation = wheel.Vehicle.Body.orientation; } System.Numerics.Vector3 linearVelocity; Vector3Ex.Subtract(ref newPosition, ref wheel.vehicle.Body.position, out linearVelocity); Vector3Ex.Cross(ref linearVelocity, ref wheel.vehicle.Body.angularVelocity, out linearVelocity); Vector3Ex.Add(ref linearVelocity, ref wheel.vehicle.Body.linearVelocity, out linearVelocity); detector.LinearVelocity = linearVelocity; detector.AngularVelocity = wheel.vehicle.Body.angularVelocity; }
public TriangleShape(Vector3 vertex0, Vector3 vertex1, Vector3 vertex2, MaterialAbstract material) { Vector3 planeNormal = Vector3Ex.Cross(vertex1 - vertex0, vertex2 - vertex0).GetNormal(); double distanceFromOrigin = Vector3Ex.Dot(vertex0, planeNormal); Plane = new PlaneFloat(new Vector3Float(planeNormal), (float)distanceFromOrigin); Material = material; vertices[0] = new Vector3Float(vertex0); vertices[1] = new Vector3Float(vertex1); vertices[2] = new Vector3Float(vertex2); center = new Vector3Float((vertex0 + vertex1 + vertex2) / 3); var normalLengths = new [] { Math.Abs(planeNormal.X), Math.Abs(planeNormal.Y), Math.Abs(planeNormal.Z) }; MajorAxis = (byte)normalLengths.Select((v, i) => new { Axis = i, Value = Math.Abs(v) }).OrderBy(o => o.Value).Last().Axis; for (int i = 0; i < 3; i++) { boundsOnMajorAxis.Left = Math.Min(vertices[i][xForMajorAxis], boundsOnMajorAxis.Left); boundsOnMajorAxis.Right = Math.Max(vertices[i][xForMajorAxis], boundsOnMajorAxis.Right); boundsOnMajorAxis.Bottom = Math.Min(vertices[i][yForMajorAxis], boundsOnMajorAxis.Bottom); boundsOnMajorAxis.Top = Math.Max(vertices[i][yForMajorAxis], boundsOnMajorAxis.Top); } aabbMinXYZ = vertices[0].ComponentMin(vertices[1]).ComponentMin(vertices[2]); aabbMaxXYZ = vertices[0].ComponentMax(vertices[1]).ComponentMax(vertices[2]); }
///<summary> /// Updates the time of impact for the pair. ///</summary> ///<param name="requester">Collidable requesting the update.</param> ///<param name="dt">Timestep duration.</param> public override void UpdateTimeOfImpact(Collidable requester, float dt) { //Notice that we don't test for convex entity null explicitly. The convex.IsActive property does that for us. if (convex.IsActive && convex.entity.PositionUpdateMode == PositionUpdateMode.Continuous) { //TODO: This system could be made more robust by using a similar region-based rejection of edges. //CCD events are awfully rare under normal circumstances, so this isn't usually an issue. //Only perform the test if the minimum radii are small enough relative to the size of the velocity. System.Numerics.Vector3 velocity; Vector3Ex.Multiply(ref convex.entity.linearVelocity, dt, out velocity); float velocitySquared = velocity.LengthSquared(); var minimumRadius = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling; timeOfImpact = 1; if (minimumRadius * minimumRadius < velocitySquared) { var triangle = PhysicsThreadResources.GetTriangle(); triangle.collisionMargin = 0; //Spherecast against all triangles to find the earliest time. for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++) { mesh.Shape.TriangleMeshData.GetTriangle(MeshManifold.overlappedTriangles.Elements[i], out triangle.vA, out triangle.vB, out triangle.vC); //Put the triangle into 'localish' space of the convex. Vector3Ex.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA); Vector3Ex.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB); Vector3Ex.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC); RayHit rayHit; if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) && rayHit.T > Toolbox.BigEpsilon) { if (mesh.sidedness != TriangleSidedness.DoubleSided) { System.Numerics.Vector3 AB, AC; Vector3Ex.Subtract(ref triangle.vB, ref triangle.vA, out AB); Vector3Ex.Subtract(ref triangle.vC, ref triangle.vA, out AC); System.Numerics.Vector3 normal; Vector3Ex.Cross(ref AB, ref AC, out normal); float dot; Vector3Ex.Dot(ref normal, ref rayHit.Normal, out dot); //Only perform sweep if the object is in danger of hitting the object. //Triangles can be one sided, so check the impact normal against the triangle normal. if (mesh.sidedness == TriangleSidedness.Counterclockwise && dot < 0 || mesh.sidedness == TriangleSidedness.Clockwise && dot > 0) { timeOfImpact = rayHit.T; } } else { timeOfImpact = rayHit.T; } } } PhysicsThreadResources.GiveBack(triangle); } } }
/// <summary> /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// </summary> public override void ExclusiveUpdate() { //"Warm start" the constraint by applying a first guess of the solution should be. entity.ApplyLinearImpulse(ref accumulatedImpulse); System.Numerics.Vector3 taImpulse; Vector3Ex.Cross(ref r, ref accumulatedImpulse, out taImpulse); entity.ApplyAngularImpulse(ref taImpulse); }
//public static void GetInertiaOffset(System.Numerics.Vector3 offset, float mass, out Matrix3x3 additionalInertia) //{ // additionalInertia.M11 = mass * (offset.Y * offset.Y + offset.Z * offset.Z); // additionalInertia.M12 = -mass * offset.X * offset.Y; // additionalInertia.M13 = -mass * offset.X * offset.Z; // additionalInertia.M21 = -mass * offset.Y * offset.X; // additionalInertia.M22 = mass * (offset.X * offset.X + offset.Z * offset.Z); // additionalInertia.M23 = -mass * offset.Y * offset.Z; // additionalInertia.M31 = -mass * offset.Z * offset.X; // additionalInertia.M32 = -mass * offset.Z * offset.Y; // additionalInertia.M33 = mass * (offset.X * offset.X + offset.Y * offset.Y); //} /// <summary> /// Computes a minimum radius estimate of a shape based on a convex mesh representation. /// </summary> /// <param name="vertices">Vertices of the convex mesh.</param> /// <param name="triangleIndices">Groups of 3 indices into the vertices array which represent the triangles of the convex mesh.</param> /// <param name="center">Center of the convex shape.</param> public static float ComputeMinimumRadius(IList <System.Numerics.Vector3> vertices, IList <int> triangleIndices, ref System.Numerics.Vector3 center) { //Walk through all of the triangles. Treat them as a bunch of planes which bound the shape. //The closest distance on any of those planes to the center is the radius of the largest sphere, //centered on the... center, which can fit in the shape. //While this shares a lot of math with the volume distribution computation (volume of a parallelepiped), //it requires that a center be available. So, it's a separate calculation. float minimumDistance = float.MaxValue; for (int i = 0; i < triangleIndices.Count; i += 3) { System.Numerics.Vector3 v2 = vertices[triangleIndices[i]]; System.Numerics.Vector3 v3 = vertices[triangleIndices[i + 1]]; System.Numerics.Vector3 v4 = vertices[triangleIndices[i + 2]]; //This normal calculation creates a dependency on winding. //It needs to be consistent with the SampleDirections triangle winding. System.Numerics.Vector3 v2v3, v2v4; Vector3Ex.Subtract(ref v3, ref v2, out v2v3); Vector3Ex.Subtract(ref v4, ref v2, out v2v4); System.Numerics.Vector3 normal; Vector3Ex.Cross(ref v2v4, ref v2v3, out normal); //Watch out: this could very easily be a degenerate triangle; the sampling approach tends to create them. float lengthSquared = normal.LengthSquared(); if (lengthSquared > 1e-10f) { Vector3Ex.Divide(ref normal, (float)Math.Sqrt(lengthSquared), out normal); } else { continue; } System.Numerics.Vector3 fromCenterToPlane; Vector3Ex.Subtract(ref v2, ref center, out fromCenterToPlane); float distance; Vector3Ex.Dot(ref normal, ref fromCenterToPlane, out distance); if (distance < 0) { throw new ArgumentException("Invalid distance. Ensure the mesh is convex, has consistent winding, and contains the passed-in center."); } if (distance < minimumDistance) { minimumDistance = distance; } } return(minimumDistance); //Technically, we could also compute a maximum radius estimate... //but that amounts to finding the furthest distance contained by the set of planes defined by the sampled extreme points and their associated sample directions. //That's a trickier thing to compute quickly, and it's not all that important to make the estimate ultra tight. }
public void CrossProduct() { var test1 = new Vector3(10, 0, 0); var test2 = new Vector3(1, 1, 0); Vector3 crossResult = Vector3Ex.Cross(test2, test1); Assert.IsTrue(crossResult.X == 0); Assert.IsTrue(crossResult.Y == 0); Assert.IsTrue(crossResult.Z < 0); }
public void CrossProduct() { Vector3 Test1 = new Vector3(10, 0, 0); Vector3 Test2 = new Vector3(1, 1, 0); Vector3 CrossResult = Vector3Ex.Cross(Test2, Test1); Assert.IsTrue(CrossResult.X == 0); Assert.IsTrue(CrossResult.Y == 0); Assert.IsTrue(CrossResult.Z < 0); }
/// <summary> /// Performs any per-frame computation needed by the constraint. /// </summary> /// <param name="dt">Time step duration.</param> public override void Update(float dt) { //Collect references, pick the mode, and configure the coefficients to be used by the solver. if (supportData.SupportObject != null) { //Get an easy reference to the support. var support = supportData.SupportObject as EntityCollidable; if (support != null) { supportEntity = support.Entity; } else { supportEntity = null; } } else { supportEntity = null; } maximumForce = maximumGlueForce * dt; //If we don't allow the character to get out of the ground, it could apply some significant forces to a dynamic support object. //Let the character escape penetration in a controlled manner. This mirrors the regular penetration recovery speed. //Since the vertical motion constraint works in the opposite direction of the contact penetration constraint, //this actually eliminates the 'bounce' that can occur with non-character objects in deep penetration. permittedVelocity = Math.Min(Math.Max(supportData.Depth * CollisionResponseSettings.PenetrationRecoveryStiffness / dt, 0), CollisionResponseSettings.MaximumPenetrationRecoverySpeed); //Compute the jacobians and effective mass matrix. This constraint works along a single degree of freedom, so the mass matrix boils down to a scalar. linearJacobianA = supportData.Normal; Vector3Ex.Negate(ref linearJacobianA, out linearJacobianB); float inverseEffectiveMass = characterBody.InverseMass; if (supportEntity != null) { System.Numerics.Vector3 offsetB = supportData.Position - supportEntity.Position; Vector3Ex.Cross(ref offsetB, ref linearJacobianB, out angularJacobianB); if (supportEntity.IsDynamic) { //Only dynamic entities can actually contribute anything to the effective mass. //Kinematic entities have infinite mass and inertia, so this would all zero out. Matrix3x3 inertiaInverse = supportEntity.InertiaTensorInverse; System.Numerics.Vector3 angularComponentB; Matrix3x3.Transform(ref angularJacobianB, ref inertiaInverse, out angularComponentB); float effectiveMassContribution; Vector3Ex.Dot(ref angularComponentB, ref angularJacobianB, out effectiveMassContribution); inverseEffectiveMass += supportForceFactor * (effectiveMassContribution + supportEntity.InverseMass); } } effectiveMass = 1f / (inverseEffectiveMass); //So much nicer and shorter than the horizontal constraint! }
/// <summary> /// Calculates and applies corrective impulses. /// Called automatically by space. /// </summary> public override float SolveIteration() { #if !WINDOWS System.Numerics.Vector3 lambda = new System.Numerics.Vector3(); #else System.Numerics.Vector3 lambda; #endif //Velocity along the length. System.Numerics.Vector3 cross; System.Numerics.Vector3 aVel, bVel; Vector3Ex.Cross(ref connectionA.angularVelocity, ref worldOffsetA, out cross); Vector3Ex.Add(ref connectionA.linearVelocity, ref cross, out aVel); Vector3Ex.Cross(ref connectionB.angularVelocity, ref worldOffsetB, out cross); Vector3Ex.Add(ref connectionB.linearVelocity, ref cross, out bVel); lambda.X = aVel.X - bVel.X + biasVelocity.X - softness * accumulatedImpulse.X; lambda.Y = aVel.Y - bVel.Y + biasVelocity.Y - softness * accumulatedImpulse.Y; lambda.Z = aVel.Z - bVel.Z + biasVelocity.Z - softness * accumulatedImpulse.Z; //Turn the velocity into an impulse. Matrix3x3.Transform(ref lambda, ref massMatrix, out lambda); //ACcumulate the impulse Vector3Ex.Add(ref accumulatedImpulse, ref lambda, out accumulatedImpulse); //Apply the impulse //Constraint.applyImpulse(myConnectionA, myConnectionB, ref rA, ref rB, ref impulse); #if !WINDOWS System.Numerics.Vector3 linear = new System.Numerics.Vector3(); #else System.Numerics.Vector3 linear; #endif if (connectionA.isDynamic) { linear.X = -lambda.X; linear.Y = -lambda.Y; linear.Z = -lambda.Z; connectionA.ApplyLinearImpulse(ref linear); System.Numerics.Vector3 taImpulse; Vector3Ex.Cross(ref worldOffsetA, ref linear, out taImpulse); connectionA.ApplyAngularImpulse(ref taImpulse); } if (connectionB.isDynamic) { connectionB.ApplyLinearImpulse(ref lambda); System.Numerics.Vector3 tbImpulse; Vector3Ex.Cross(ref worldOffsetB, ref lambda, out tbImpulse); connectionB.ApplyAngularImpulse(ref tbImpulse); } return(Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z)); }
///<summary> /// Gets the normal of the triangle shape in its local space. ///</summary> ///<returns>The local normal.</returns> public System.Numerics.Vector3 GetLocalNormal() { System.Numerics.Vector3 normal; System.Numerics.Vector3 vAvB; System.Numerics.Vector3 vAvC; Vector3Ex.Subtract(ref vB, ref vA, out vAvB); Vector3Ex.Subtract(ref vC, ref vA, out vAvC); Vector3Ex.Cross(ref vAvB, ref vAvC, out normal); normal.Normalize(); return(normal); }
public override (double u, double v) GetUv(IntersectInfo info) { Vector3 Position = plane.Normal; Vector3 vecU = new Vector3(Position.Y, Position.Z, -Position.X); Vector3 vecV = Vector3Ex.Cross(vecU, plane.Normal); double u = Vector3Ex.Dot(info.HitPosition, vecU); double v = Vector3Ex.Dot(info.HitPosition, vecV); return(u, v); }
protected internal override void UpdateJacobiansAndVelocityBias() { //Transform the anchors and offsets into world space. System.Numerics.Vector3 offsetA, offsetB; QuaternionEx.Transform(ref LocalAnchorA, ref ConnectionA.Orientation, out offsetA); QuaternionEx.Transform(ref LocalAnchorB, ref ConnectionB.Orientation, out offsetB); System.Numerics.Vector3 anchorA, anchorB; Vector3Ex.Add(ref ConnectionA.Position, ref offsetA, out anchorA); Vector3Ex.Add(ref ConnectionB.Position, ref offsetB, out anchorB); //Compute the distance. System.Numerics.Vector3 separation; Vector3Ex.Subtract(ref anchorB, ref anchorA, out separation); float currentDistance = separation.Length(); //Compute jacobians System.Numerics.Vector3 linearA; #if !WINDOWS linearA = new System.Numerics.Vector3(); #endif if (currentDistance > Toolbox.Epsilon) { linearA.X = separation.X / currentDistance; linearA.Y = separation.Y / currentDistance; linearA.Z = separation.Z / currentDistance; velocityBias = new System.Numerics.Vector3(errorCorrectionFactor * (currentDistance - distance), 0, 0); } else { velocityBias = new System.Numerics.Vector3(); linearA = new System.Numerics.Vector3(); } System.Numerics.Vector3 angularA, angularB; Vector3Ex.Cross(ref offsetA, ref linearA, out angularA); //linearB = -linearA, so just swap the cross product order. Vector3Ex.Cross(ref linearA, ref offsetB, out angularB); //Put all the 1x3 jacobians into a 3x3 matrix representation. linearJacobianA = new Matrix3x3 { M11 = linearA.X, M12 = linearA.Y, M13 = linearA.Z }; linearJacobianB = new Matrix3x3 { M11 = -linearA.X, M12 = -linearA.Y, M13 = -linearA.Z }; angularJacobianA = new Matrix3x3 { M11 = angularA.X, M12 = angularA.Y, M13 = angularA.Z }; angularJacobianB = new Matrix3x3 { M11 = angularB.X, M12 = angularB.Y, M13 = angularB.Z }; }
protected internal override void GetContactInformation(int index, out ContactInformation info) { info.Contact = ContactManifold.contacts.Elements[index]; //Find the contact's normal force. float totalNormalImpulse = 0; info.NormalImpulse = 0; for (int i = 0; i < contactConstraint.penetrationConstraints.Count; i++) { totalNormalImpulse += contactConstraint.penetrationConstraints.Elements[i].accumulatedImpulse; if (contactConstraint.penetrationConstraints.Elements[i].contact == info.Contact) { info.NormalImpulse = contactConstraint.penetrationConstraints.Elements[i].accumulatedImpulse; } } //Compute friction force. Since we are using central friction, this is 'faked.' float radius; Vector3Ex.Distance(ref contactConstraint.slidingFriction.manifoldCenter, ref info.Contact.Position, out radius); if (totalNormalImpulse > 0) { info.FrictionImpulse = (info.NormalImpulse / totalNormalImpulse) * (contactConstraint.slidingFriction.accumulatedImpulse.Length() + contactConstraint.twistFriction.accumulatedImpulse * radius); } else { info.FrictionImpulse = 0; } //Compute relative velocity System.Numerics.Vector3 velocity; //If the pair is handling some type of query and does not actually have supporting entities, then consider the velocity contribution to be zero. if (EntityA != null) { Vector3Ex.Subtract(ref info.Contact.Position, ref EntityA.position, out velocity); Vector3Ex.Cross(ref EntityA.angularVelocity, ref velocity, out velocity); Vector3Ex.Add(ref velocity, ref EntityA.linearVelocity, out info.RelativeVelocity); } else { info.RelativeVelocity = new System.Numerics.Vector3(); } if (EntityB != null) { Vector3Ex.Subtract(ref info.Contact.Position, ref EntityB.position, out velocity); Vector3Ex.Cross(ref EntityB.angularVelocity, ref velocity, out velocity); Vector3Ex.Add(ref velocity, ref EntityB.linearVelocity, out velocity); Vector3Ex.Subtract(ref info.RelativeVelocity, ref velocity, out info.RelativeVelocity); } info.Pair = this; }
public void CrossProduct() { Random Rand = new Random(); Vector2 TestVector2D1 = new Vector2(Rand.NextDouble() * 1000, Rand.NextDouble() * 1000); Vector2 TestVector2D2 = new Vector2(Rand.NextDouble() * 1000, Rand.NextDouble() * 1000); double Cross2D = Vector2.Cross(TestVector2D1, TestVector2D2); Vector3 TestVector31 = new Vector3(TestVector2D1.X, TestVector2D1.Y, 0); Vector3 TestVector32 = new Vector3(TestVector2D2.X, TestVector2D2.Y, 0); Vector3 Cross3D = Vector3Ex.Cross(TestVector31, TestVector32); Assert.IsTrue(Cross3D.Z == Cross2D); }
public void CrossProduct() { var rand = new Random(); var testVector2D1 = new Vector2(rand.NextDouble() * 1000, rand.NextDouble() * 1000); var testVector2D2 = new Vector2(rand.NextDouble() * 1000, rand.NextDouble() * 1000); double cross2D = Vector2.Cross(testVector2D1, testVector2D2); var testVector31 = new Vector3(testVector2D1.X, testVector2D1.Y, 0); var testVector32 = new Vector3(testVector2D2.X, testVector2D2.Y, 0); Vector3 cross3D = Vector3Ex.Cross(testVector31, testVector32); Assert.IsTrue(cross3D.Z == cross2D); }
/// <summary> /// Computes one iteration of the constraint to meet the solver updateable's goal. /// </summary> /// <returns>The rough applied impulse magnitude.</returns> public override float SolveIteration() { //TODO: This could technically be faster. //Form the jacobian explicitly. //Cross cross add add subtract dot //vs //dot dot dot dot and then scalar adds System.Numerics.Vector3 dv; System.Numerics.Vector3 aVel, bVel; Vector3Ex.Cross(ref connectionA.angularVelocity, ref rA, out aVel); Vector3Ex.Add(ref aVel, ref connectionA.linearVelocity, out aVel); Vector3Ex.Cross(ref connectionB.angularVelocity, ref rB, out bVel); Vector3Ex.Add(ref bVel, ref connectionB.linearVelocity, out bVel); Vector3Ex.Subtract(ref aVel, ref bVel, out dv); float velocityDifference; Vector3Ex.Dot(ref dv, ref worldPlaneNormal, out velocityDifference); //if(velocityDifference > 0) // Debug.WriteLine("Velocity difference: " + velocityDifference); //Debug.WriteLine("softness velocity: " + softness * accumulatedImpulse); float lambda = negativeEffectiveMass * (velocityDifference + biasVelocity + softness * accumulatedImpulse); accumulatedImpulse += lambda; System.Numerics.Vector3 impulse; System.Numerics.Vector3 torque; Vector3Ex.Multiply(ref worldPlaneNormal, lambda, out impulse); if (connectionA.isDynamic) { Vector3Ex.Multiply(ref rAcrossN, lambda, out torque); connectionA.ApplyLinearImpulse(ref impulse); connectionA.ApplyAngularImpulse(ref torque); } if (connectionB.isDynamic) { Vector3Ex.Negate(ref impulse, out impulse); Vector3Ex.Multiply(ref rBcrossN, lambda, out torque); connectionB.ApplyLinearImpulse(ref impulse); connectionB.ApplyAngularImpulse(ref torque); } return(lambda); }
void ComputeRestrictedAxes() { System.Numerics.Vector3 cross; Vector3Ex.Cross(ref localLineDirection, ref Toolbox.UpVector, out cross); float lengthSquared = cross.LengthSquared(); if (lengthSquared > Toolbox.Epsilon) { Vector3Ex.Divide(ref cross, (float)Math.Sqrt(lengthSquared), out localRestrictedAxis1); } else { //Oops! The direction is aligned with the up vector. Vector3Ex.Cross(ref localLineDirection, ref Toolbox.RightVector, out cross); Vector3Ex.Normalize(ref cross, out localRestrictedAxis1); } //Don't need to normalize this; cross product of two unit length perpendicular vectors. Vector3Ex.Cross(ref localRestrictedAxis1, ref localLineDirection, out localRestrictedAxis2); }
protected internal override void UpdateJacobiansAndVelocityBias() { //Transform the anchors and offsets into world space. System.Numerics.Vector3 offsetA, offsetB, lineDirection; QuaternionEx.Transform(ref LocalPlaneAnchor, ref ConnectionA.Orientation, out offsetA); QuaternionEx.Transform(ref LocalPlaneNormal, ref ConnectionA.Orientation, out lineDirection); QuaternionEx.Transform(ref LocalAnchorB, ref ConnectionB.Orientation, out offsetB); System.Numerics.Vector3 anchorA, anchorB; Vector3Ex.Add(ref ConnectionA.Position, ref offsetA, out anchorA); Vector3Ex.Add(ref ConnectionB.Position, ref offsetB, out anchorB); //Compute the distance. System.Numerics.Vector3 separation; Vector3Ex.Subtract(ref anchorB, ref anchorA, out separation); //This entire constraint is very similar to the IKDistanceLimit, except the current distance is along an axis. float currentDistance; Vector3Ex.Dot(ref separation, ref lineDirection, out currentDistance); velocityBias = new System.Numerics.Vector3(errorCorrectionFactor * currentDistance, 0, 0); //Compute jacobians System.Numerics.Vector3 angularA, angularB; //We can't just use the offset to anchor for A's jacobian- the 'collision' location is way out there at anchorB! System.Numerics.Vector3 rA; Vector3Ex.Subtract(ref anchorB, ref ConnectionA.Position, out rA); Vector3Ex.Cross(ref rA, ref lineDirection, out angularA); //linearB = -linearA, so just swap the cross product order. Vector3Ex.Cross(ref lineDirection, ref offsetB, out angularB); //Put all the 1x3 jacobians into a 3x3 matrix representation. linearJacobianA = new Matrix3x3 { M11 = lineDirection.X, M12 = lineDirection.Y, M13 = lineDirection.Z }; linearJacobianB = new Matrix3x3 { M11 = -lineDirection.X, M12 = -lineDirection.Y, M13 = -lineDirection.Z }; angularJacobianA = new Matrix3x3 { M11 = angularA.X, M12 = angularA.Y, M13 = angularA.Z }; angularJacobianB = new Matrix3x3 { M11 = angularB.X, M12 = angularB.Y, M13 = angularB.Z }; }
protected internal override void UpdateJacobiansAndVelocityBias() { linearJacobianA = linearJacobianB = new Matrix3x3(); //There are two free axes and one restricted axis. //The constraint attempts to keep the hinge axis attached to connection A and the twist axis attached to connection B perpendicular to each other. //The restricted axis is the cross product between the twist and hinge axes. System.Numerics.Vector3 worldTwistAxis, worldHingeAxis; QuaternionEx.Transform(ref LocalHingeAxis, ref ConnectionA.Orientation, out worldHingeAxis); QuaternionEx.Transform(ref LocalTwistAxis, ref ConnectionB.Orientation, out worldTwistAxis); System.Numerics.Vector3 restrictedAxis; Vector3Ex.Cross(ref worldHingeAxis, ref worldTwistAxis, out restrictedAxis); //Attempt to normalize the restricted axis. float lengthSquared = restrictedAxis.LengthSquared(); if (lengthSquared > Toolbox.Epsilon) { Vector3Ex.Divide(ref restrictedAxis, (float)Math.Sqrt(lengthSquared), out restrictedAxis); } else { restrictedAxis = new System.Numerics.Vector3(); } angularJacobianA = new Matrix3x3 { M11 = restrictedAxis.X, M12 = restrictedAxis.Y, M13 = restrictedAxis.Z, }; Matrix3x3.Negate(ref angularJacobianA, out angularJacobianB); float error; Vector3Ex.Dot(ref worldHingeAxis, ref worldTwistAxis, out error); error = (float)Math.Acos(MathHelper.Clamp(error, -1, 1)) - MathHelper.PiOver2; velocityBias = new System.Numerics.Vector3(errorCorrectionFactor * error, 0, 0); }
//TODO: Having a specialized triangle-triangle pair test would be nice. Even if it didn't use an actual triangle-triangle test, certain assumptions could still make it speedier and more elegant. //"Closest points between triangles" + persistent manifolding would probably be the best approach (a lot faster than the triangle-convex general case anyway). public override bool GenerateContactCandidates(TriangleShape triangle, out TinyStructList <ContactData> contactList) { if (base.GenerateContactCandidates(triangle, out contactList)) { //The triangle-convex pair test has already rejected contacts whose normals would violate the first triangle's sidedness. //However, since it's a vanilla triangle-convex test, it doesn't know about the sidedness of the other triangle! var shape = ((TriangleShape)convex); System.Numerics.Vector3 normal; //Lots of recalculating ab-bc! System.Numerics.Vector3 ab, ac; Vector3Ex.Subtract(ref shape.vB, ref shape.vA, out ab); Vector3Ex.Subtract(ref shape.vC, ref shape.vA, out ac); Vector3Ex.Cross(ref ab, ref ac, out normal); var sidedness = shape.sidedness; if (sidedness != TriangleSidedness.DoubleSided) { for (int i = contactList.Count - 1; i >= 0; i--) { ContactData item; contactList.Get(i, out item); float dot; Vector3Ex.Dot(ref item.Normal, ref normal, out dot); if (sidedness == TriangleSidedness.Clockwise) { if (dot < 0) { contactList.RemoveAt(i); } } else { if (dot > 0) { contactList.RemoveAt(i); } } } } return(contactList.Count > 0); } return(false); }
/// <summary> /// Computes one iteration of the constraint to meet the solver updateable's goal. /// </summary> /// <returns>The rough applied impulse magnitude.</returns> public override float SolveIteration() { //Compute relative velocity System.Numerics.Vector3 lambda; Vector3Ex.Cross(ref r, ref entity.angularVelocity, out lambda); Vector3Ex.Subtract(ref lambda, ref entity.linearVelocity, out lambda); //Add in bias velocity Vector3Ex.Add(ref biasVelocity, ref lambda, out lambda); //Add in softness System.Numerics.Vector3 softnessVelocity; Vector3Ex.Multiply(ref accumulatedImpulse, usedSoftness, out softnessVelocity); Vector3Ex.Subtract(ref lambda, ref softnessVelocity, out lambda); //In terms of an impulse (an instantaneous change in momentum), what is it? Matrix3x3.Transform(ref lambda, ref effectiveMassMatrix, out lambda); //Sum the impulse. System.Numerics.Vector3 previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse += lambda; //If the impulse it takes to get to the goal is too high for the motor to handle, scale it back. float sumImpulseLengthSquared = accumulatedImpulse.LengthSquared(); if (sumImpulseLengthSquared > maxForceDtSquared) { //max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum. accumulatedImpulse *= maxForceDt / (float)Math.Sqrt(sumImpulseLengthSquared); //Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained. lambda = accumulatedImpulse - previousAccumulatedImpulse; } entity.ApplyLinearImpulse(ref lambda); System.Numerics.Vector3 taImpulse; Vector3Ex.Cross(ref r, ref lambda, out taImpulse); entity.ApplyAngularImpulse(ref taImpulse); return(Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z)); }