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));
}
