/// <summary>
/// Applies the sequential impulse.
/// </summary>
public override float SolveIteration()
{
float lambda;
System.Numerics.Vector3 relativeVelocity;
Vector3Ex.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out relativeVelocity);
//Transform the velocity to with the jacobian
Vector3Ex.Dot(ref relativeVelocity, ref hingeAxis, out lambda);
//Add in the constraint space bias velocity
lambda = -lambda + biasVelocity - softness * accumulatedImpulse;
//Transform to an impulse
lambda *= velocityToImpulse;
//Clamp accumulated impulse (can't go negative)
float previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = MathHelper.Max(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
//Apply the impulse
System.Numerics.Vector3 impulse;
Vector3Ex.Multiply(ref hingeAxis, lambda, out impulse);
if (connectionA.isDynamic)
{
connectionA.ApplyAngularImpulse(ref impulse);
}
if (connectionB.isDynamic)
{
Vector3Ex.Negate(ref impulse, out impulse);
connectionB.ApplyAngularImpulse(ref impulse);
}
return(Math.Abs(lambda));
}
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);
}
private Ray GetRefractionRay(Vector3 P, Vector3 N, Vector3 V, double refraction)
{
int method = 0;
switch (method)
{
case 0:
return(new Ray(P, V, Ray.sameSurfaceOffset, double.MaxValue));
case 1:
V = V * -1;
double n = -0.55; // refraction constant for now
if (n < 0 || n > 1)
{
return(new Ray(P, V)); // no refraction
}
break;
case 2:
double c1 = Vector3Ex.Dot(N, V);
double c2 = 1 - refraction * refraction * (1 - c1 * c1);
if (c2 < 0)
{
c2 = Math.Sqrt(c2);
}
Vector3 T = (N * (refraction * c1 - c2) - V * refraction) * -1;
T.Normalize();
return(new Ray(P, T)); // no refraction
}
return(new Ray(P, V, Ray.sameSurfaceOffset, double.MaxValue));
}
public override void OnMouseDown(Mouse3DEventArgs mouseEvent3D)
{
if (mouseEvent3D.info != null && Object3DControlContext.Scene.SelectedItem != null)
{
hadClickOnControl = true;
activeSelectedItem = RootSelection;
zValueDisplayInfo.Visible = true;
var selectedItem = activeSelectedItem;
double distanceToHit = Vector3Ex.Dot(mouseEvent3D.info.HitPosition, mouseEvent3D.MouseRay.directionNormal);
hitPlane = new PlaneShape(mouseEvent3D.MouseRay.directionNormal, distanceToHit, null);
originalPointToMove = GetTopPosition(selectedItem);
initialHitPosition = mouseEvent3D.info.HitPosition;
if (selectedItem is IObjectWithHeight heightObject)
{
heightOnMouseDown = heightObject.Height;
}
mouseDownSelectedBounds = selectedItem.GetAxisAlignedBoundingBox();
}
base.OnMouseDown(mouseEvent3D);
}
private Ray GetReflectionRay(Vector3 p, Vector3 n, Vector3 v)
{
double c1 = -Vector3Ex.Dot(n, v);
Vector3 rl = v + (n * 2 * c1);
return(new Ray(p, rl, Ray.sameSurfaceOffset, double.MaxValue));
}
/// <summary>
/// Compute the point resulting from the intersection with a plane
/// </summary>
/// <param name="normal">the plane normal</param>
/// <param name="planePoint">a plane point.</param>
/// <returns>intersection point.If they don't intersect, return null</returns>
public Vector3 ComputePlaneIntersection(Plane plane)
{
double distanceToStartFromOrigin = Vector3Ex.Dot(plane.Normal, startPoint);
double distanceFromPlane = distanceToStartFromOrigin - plane.DistanceFromOrigin;
double denominator = Vector3Ex.Dot(plane.Normal, Direction);
if (Math.Abs(denominator) < EqualityTolerance)
{
//if line is parallel to the plane...
if (Math.Abs(distanceFromPlane) < EqualityTolerance)
{
//if line is contained in the plane...
return(startPoint);
}
else
{
return(Vector3.PositiveInfinity);
}
}
else // line intercepts the plane...
{
double t = -distanceFromPlane / denominator;
Vector3 resultPoint = new Vector3();
resultPoint.X = startPoint.X + t * Direction.X;
resultPoint.Y = startPoint.Y + t * Direction.Y;
resultPoint.Z = startPoint.Z + t * Direction.Z;
return(resultPoint);
}
}
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 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);
}
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>
/// 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()
{
float velocityA, velocityB;
//Find the velocity contribution from each connection
Vector3Ex.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA);
Vector3Ex.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB);
//Add in the constraint space bias velocity
float lambda = (-velocityA - velocityB) - biasVelocity - softness * accumulatedImpulse;
//Transform to an impulse
lambda *= velocityToImpulse;
//Clamp accumulated impulse (can't go negative)
float previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = MathHelper.Min(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
//Apply the impulse
System.Numerics.Vector3 impulse;
if (connectionA.isDynamic)
{
Vector3Ex.Multiply(ref jacobianA, lambda, out impulse);
connectionA.ApplyAngularImpulse(ref impulse);
}
if (connectionB.isDynamic)
{
Vector3Ex.Multiply(ref jacobianB, lambda, out impulse);
connectionB.ApplyAngularImpulse(ref impulse);
}
return(Math.Abs(lambda));
}
private Ray GetRefractionRay(Vector3 p, Vector3 n, Vector3 v, double refraction)
{
int method = 0;
switch (method)
{
case 0:
return(new Ray(p, v, Ray.sameSurfaceOffset, double.MaxValue));
case 1:
v *= -1;
double newRefraction = -0.55; // refraction constant for now
if (newRefraction < 0 || newRefraction > 1)
{
return(new Ray(p, v)); // no refraction
}
break;
case 2:
double c1 = Vector3Ex.Dot(n, v);
double c2 = 1 - refraction * refraction * (1 - c1 * c1);
if (c2 < 0)
{
c2 = Math.Sqrt(c2);
}
Vector3 t = (n * (refraction * c1 - c2) - v * refraction) * -1;
t.Normalize();
return(new Ray(p, t)); // no refraction
}
return(new Ray(p, v, Ray.sameSurfaceOffset, double.MaxValue));
}
bool IsObstructiveToUpStepping(ref System.Numerics.Vector3 sideNormal, ref ContactData contact)
{
//Up stepping has slightly different rules than down stepping.
//For contacts with normals aligned with the side normal that triggered the step,
//only marginal (allowed penetration) obstruction is permitted.
//Consider the side normal to define an implicit plane.
float dot;
Vector3Ex.Dot(ref contact.Normal, ref sideNormal, out dot);
if (dot * contact.PenetrationDepth > CollisionDetectionSettings.AllowedPenetration)
{
//It's too deep! Can't step.
return(true);
}
//Go through side-facing contact and check to see if the new contact is deeper than any existing contact in the direction of the existing contact.
//This is equivalent to considering the existing contacts to define planes and then comparing the new contact against those planes.
//Since we already have the penetration depths, we don't need to use the positions of the contacts.
foreach (var c in SupportFinder.SideContacts)
{
dot = Vector3Ex.Dot(contact.Normal, c.Contact.Normal);
float depth = dot * c.Contact.PenetrationDepth;
if (depth > Math.Max(c.Contact.PenetrationDepth, CollisionDetectionSettings.AllowedPenetration))
{
return(true);
}
}
return(false);
}
/// <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.
//Technically, there exists a better estimate of the necessary speed, but choosing the maximum position correction speed is a nice catch-all.
//If you change that correction speed, watch out!!! It could significantly change the way the character behaves when trying to glue to surfaces.
if (supportData.Depth > 0)
{
permittedVelocity = CollisionResponseSettings.MaximumPositionCorrectionSpeed;
}
else
{
permittedVelocity = 0;
}
//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;
Vector3.Negate(ref linearJacobianA, out linearJacobianB);
effectiveMass = character.Body.InverseMass;
if (supportEntity != null)
{
Vector3 offsetB = supportData.Position - supportEntity.Position;
Vector3.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.LocalInertiaTensorInverse;
Vector3 angularComponentB;
Matrix3X3.Transform(ref angularJacobianB, ref inertiaInverse, out angularComponentB);
float effectiveMassContribution;
Vector3Ex.Dot(ref angularComponentB, ref angularJacobianB, out effectiveMassContribution);
effectiveMass += supportForceFactor * (effectiveMassContribution + supportEntity.InverseMass);
}
}
effectiveMass = 1 / effectiveMass;
//So much nicer and shorter than the horizontal constraint!
}
/// <summary>
/// Solves for velocity.
/// </summary>
public override float SolveIteration()
{
float velocityA, velocityB;
//Find the velocity contribution from each connection
Vector3Ex.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA);
Vector3Ex.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB);
//Add in the constraint space bias velocity
float lambda = -(velocityA + velocityB) - biasVelocity - softness * accumulatedImpulse;
//Transform to an impulse
lambda *= velocityToImpulse;
//Accumulate the impulse
accumulatedImpulse += lambda;
//Apply the impulse
System.Numerics.Vector3 impulse;
if (connectionA.isDynamic)
{
Vector3Ex.Multiply(ref jacobianA, lambda, out impulse);
connectionA.ApplyAngularImpulse(ref impulse);
}
if (connectionB.isDynamic)
{
Vector3Ex.Multiply(ref jacobianB, lambda, out impulse);
connectionB.ApplyAngularImpulse(ref impulse);
}
return(Math.Abs(lambda));
}
private bool IsContactUnique(ref ContactData contactCandidate, ref QuickList <ContactData> candidatesToAdd)
{
contactCandidate.Validate();
float distanceSquared;
RigidTransform meshTransform = MeshTransform;
for (int i = 0; i < contacts.Count; i++)
{
Vector3Ex.DistanceSquared(ref contacts.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
{
//This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that
//a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent.
Vector3Ex.Dot(ref contacts.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared);
if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum)
{
//Update the existing 'redundant' contact with the new information.
//This works out because the new contact is the deepest contact according to the previous collision detection iteration.
contacts.Elements[i].Normal = contactCandidate.Normal;
contacts.Elements[i].Position = contactCandidate.Position;
contacts.Elements[i].PenetrationDepth = contactCandidate.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = contactCandidate.PenetrationDepth;
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref convex.worldTransform, out supplementData.Elements[i].LocalOffsetA);
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref meshTransform, out supplementData.Elements[i].LocalOffsetB);
return(false);
}
}
}
for (int i = 0; i < candidatesToAdd.Count; i++)
{
Vector3Ex.DistanceSquared(ref candidatesToAdd.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
{
//This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that
//a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent.
Vector3Ex.Dot(ref candidatesToAdd.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared);
if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum)
{
return(false);
}
}
}
//for (int i = 0; i < edgeContacts.count; i++)
//{
// Vector3Ex.DistanceSquared(ref edgeContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared);
// if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
// {
// return false;
// }
//}
//for (int i = 0; i < vertexContacts.count; i++)
//{
// Vector3Ex.DistanceSquared(ref vertexContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared);
// if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
// {
// return false;
// }
//}
return(true);
}
///<summary>
/// Gets the closest point on the segment to the origin.
///</summary>
///<param name="point">Closest point.</param>
public void GetPointOnSegmentClosestToOrigin(out System.Numerics.Vector3 point)
{
System.Numerics.Vector3 segmentDisplacement;
Vector3Ex.Subtract(ref B, ref A, out segmentDisplacement);
float dotA;
Vector3Ex.Dot(ref segmentDisplacement, ref A, out dotA);
//Inside segment.
float V = -dotA / segmentDisplacement.LengthSquared();
Vector3Ex.Multiply(ref segmentDisplacement, V, out point);
Vector3Ex.Add(ref point, ref A, out point);
//if (dotB > 0)
//{
//}
//else
//{
// //It is not possible to be anywhere but within the segment in a 'boolean' GJK, where it early outs as soon as a separating axis is found.
// //Outside B.
// //Remove current A; we're becoming a point.
// A = B;
// State = SimplexState.Point;
// point = A;
//}
//It can never be outside A!
//That would mean that the origin is LESS extreme along the search direction than our extreme point--- our search direction would not have picked that direction.
}
private Ray GetReflectionRay(Vector3 P, Vector3 N, Vector3 V)
{
double c1 = -Vector3Ex.Dot(N, V);
Vector3 Rl = V + (N * 2 * c1);
return(new Ray(P, Rl, Ray.sameSurfaceOffset, double.MaxValue));
}
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>
/// 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>
/// Computes closest distance from a vertex to a plane
/// </summary>
/// <param name="vertex">vertex used to compute the distance</param>
/// <param name="face">face representing the plane where it is contained</param>
/// <returns>the closest distance from the vertex to the plane</returns>
public double DistanceFromPlane(Vertex vertex)
{
double distToV1 = this.Plane.DistanceFromOrigin;
double distToVertex = Vector3Ex.Dot(Normal, vertex.Position);
double distFromFacePlane = distToVertex - distToV1;
return(distFromFacePlane);
}
public void DotProduct()
{
var test1 = new Vector3(10, 1, 2);
var test2 = new Vector3(1, 0, 0);
double dotResult = Vector3Ex.Dot(test2, test1);
Assert.IsTrue(dotResult == 10);
}
public void DotProduct()
{
Vector3 Test1 = new Vector3(10, 1, 2);
Vector3 Test2 = new Vector3(1, 0, 0);
double DotResult = Vector3Ex.Dot(Test2, Test1);
Assert.IsTrue(DotResult == 10);
}
//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.
}
///<summary>
/// Updates the manifold.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//First, refresh all existing contacts. This is an incremental manifold.
ContactRefresher.ContactRefresh(contacts, supplementData, ref collidableA.worldTransform, ref collidableB.worldTransform, contactIndicesToRemove);
RemoveQueuedContacts();
//Now, generate a contact between the two shapes.
ContactData contact;
if (pairTester.GenerateContactCandidate(out contact))
{
//Eliminate any old contacts which have normals which would fight with this new contact.
for (int i = 0; i < contacts.Count; ++i)
{
float normalDot;
Vector3Ex.Dot(ref contacts.Elements[i].Normal, ref contact.Normal, out normalDot);
if (normalDot < 0)
{
Remove(i);
break;
}
}
//If a contact is unique, add it to the manifold separately.
//If it is redundant, it will be used to update an existing contact... within the IsContactUnique call.
//In other words: THIS FUNCTION HAS IMPORTANT SNEAKY SIDE EFFECTS.
if (IsContactUnique(ref contact))
{
//Check if adding the new contact would overflow the manifold.
if (contacts.Count == 4)
{
//Adding that contact would overflow the manifold. Reduce to the best subset.
bool addCandidate;
ContactReducer.ReduceContacts(contacts, ref contact, contactIndicesToRemove, out addCandidate);
RemoveQueuedContacts();
if (addCandidate)
{
Add(ref contact);
}
}
else
{
//Won't overflow the manifold, so just toss it in.
Add(ref contact);
}
}
}
else
{
//No collision, clean out the manifold.
for (int i = contacts.Count - 1; i >= 0; i--)
{
Remove(i);
}
}
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and two degrees of angular freedom between two entities.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="anchor">Point around which both entities rotate.</param>
/// <param name="freeAxis">Axis around which the hinge can rotate.</param>
public RevoluteJoint(Entity connectionA, Entity connectionB, System.Numerics.Vector3 anchor, System.Numerics.Vector3 freeAxis)
{
if (connectionA == null)
{
connectionA = TwoEntityConstraint.WorldEntity;
}
if (connectionB == null)
{
connectionB = TwoEntityConstraint.WorldEntity;
}
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
AngularJoint = new RevoluteAngularJoint(connectionA, connectionB, freeAxis);
Limit = new RevoluteLimit(connectionA, connectionB);
Motor = new RevoluteMotor(connectionA, connectionB, freeAxis);
Limit.IsActive = false;
Motor.IsActive = false;
//Ensure that the base and test direction is perpendicular to the free axis.
System.Numerics.Vector3 baseAxis = anchor - connectionA.position;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
{
baseAxis = connectionB.position - anchor;
}
baseAxis -= Vector3Ex.Dot(baseAxis, freeAxis) * freeAxis;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right);
}
}
Limit.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);
Motor.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);
baseAxis = connectionB.position - anchor;
baseAxis -= Vector3Ex.Dot(baseAxis, freeAxis) * freeAxis;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right);
}
}
Limit.TestAxis = baseAxis;
Motor.TestAxis = baseAxis;
Add(BallSocketJoint);
Add(AngularJoint);
Add(Limit);
Add(Motor);
}
void AnalyzeEntry(int i)
{
var entityCollidable = broadPhaseEntries[i] as EntityCollidable;
if (entityCollidable != null && entityCollidable.IsActive && entityCollidable.entity.isDynamic && CollisionRules.collisionRuleCalculator(this, entityCollidable) <= CollisionRule.Normal)
{
bool keepGoing = false;
foreach (var tri in surfaceTriangles)
{
//Don't need to do anything if the entity is outside of the water.
if (Toolbox.IsPointInsideTriangle(ref tri[0], ref tri[1], ref tri[2], ref entityCollidable.worldTransform.Position))
{
keepGoing = true;
break;
}
}
if (!keepGoing)
{
return;
}
//The entity is submerged, apply buoyancy forces.
float submergedVolume;
System.Numerics.Vector3 submergedCenter;
GetBuoyancyInformation(entityCollidable, out submergedVolume, out submergedCenter);
if (submergedVolume > 0)
{
float fractionSubmerged = submergedVolume / entityCollidable.entity.CollisionInformation.Shape.Volume;
//Divide the volume by the density multiplier if present.
float densityMultiplier;
if (DensityMultipliers.TryGetValue(entityCollidable.entity, out densityMultiplier))
{
submergedVolume /= densityMultiplier;
}
System.Numerics.Vector3 force;
Vector3Ex.Multiply(ref upVector, -gravity * Density * dt * submergedVolume, out force);
entityCollidable.entity.ApplyImpulse(ref submergedCenter, ref force);
//Flow
if (FlowForce != 0)
{
float dot = Math.Max(Vector3Ex.Dot(entityCollidable.entity.linearVelocity, flowDirection), 0);
if (dot < MaxFlowSpeed)
{
force = Math.Min(FlowForce, (MaxFlowSpeed - dot) * entityCollidable.entity.mass) * dt * fractionSubmerged * FlowDirection;
entityCollidable.entity.ApplyLinearImpulse(ref force);
}
}
//Damping
entityCollidable.entity.ModifyLinearDamping(fractionSubmerged * LinearDamping);
entityCollidable.entity.ModifyAngularDamping(fractionSubmerged * AngularDamping);
}
}
}
/// <summary>
/// Sets up the axes of the transform and ensures that it is an orthonormal basis.
/// </summary>
/// <param name="matrix">Rotation matrix representing the three axes.
/// The matrix's backward vector is used as the primary axis.
/// The matrix's right vector is used as the x axis.</param>
public void SetLocalAxes(Matrix3x3 matrix)
{
if (Math.Abs(Vector3Ex.Dot(matrix.Backward, matrix.Right)) > Toolbox.BigEpsilon)
{
throw new ArgumentException("The axes provided to the joint transform are not perpendicular. Ensure that the specified axes form a valid constraint.");
}
localPrimaryAxis = System.Numerics.Vector3.Normalize(matrix.Backward);
localXAxis = System.Numerics.Vector3.Normalize(matrix.Right);
ComputeWorldSpaceAxes();
}
/// <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!
}
// For right cylinders, SetCenter is the center point. For non-right
// cylinders, the center is just any point on the central axis.
void SetCenter(Vector3 center)
{
this.Center = center;
CenterDotAxis = Vector3Ex.Dot(Center, CenterAxis);
if (IsRightCylinder())
{
TopPlaneCoef = CenterDotAxis + HalfHeight;
BottomPlaneCoef = -(CenterDotAxis - HalfHeight);
}
}
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);
}
