public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep,
out RayHit hit)
{
return(MPRToolbox.Sweep(castShape, Shape, ref sweep, ref Toolbox.ZeroVector, ref startingTransform,
ref worldTransform, out hit));
}
public void Multiply(RigidTransform a, RigidTransform b)
{
rot = a.rot * b.rot;
pos = a.pos + a.rot * b.pos;
}
/// <summary>
/// Gets the intersection between the capsule and the ray.
/// </summary>
/// <param name="ray">Ray to test against the capsule.</param>
/// <param name="transform">Transform of the shape.</param>
/// <param name="maximumLength">Maximum distance to travel in units of the direction vector's length.</param>
/// <param name="hit">Hit data for the raycast, if any.</param>
/// <returns>Whether or not the ray hit the target.</returns>
public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit)
{
hit = new RayHit();
Quaternion conjugate;
Quaternion.Conjugate(ref transform.Orientation, out conjugate);
Vector3 localOrigin;
Vector3.Subtract(ref ray.Position, ref transform.Position, out localOrigin);
Quaternion.Transform(ref localOrigin, ref conjugate, out localOrigin);
Vector3 localDirection;
Quaternion.Transform(ref ray.Direction, ref conjugate, out localDirection);
Vector3 normal = Toolbox.ZeroVector;
float temp, tmin = 0, tmax = maximumLength;
if (Math.Abs(localDirection.X) < Toolbox.Epsilon && (localOrigin.X < -halfWidth || localOrigin.X > halfWidth))
{
return(false);
}
float inverseDirection = 1 / localDirection.X;
float t1 = (-halfWidth - localOrigin.X) * inverseDirection;
float t2 = (halfWidth - localOrigin.X) * inverseDirection;
var tempNormal = new Vector3(-1, 0, 0);
if (t1 > t2)
{
temp = t1;
t1 = t2;
t2 = temp;
tempNormal *= -1;
}
temp = tmin;
tmin = Math.Max(tmin, t1);
if (temp != tmin)
{
normal = tempNormal;
}
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
if (Math.Abs(localDirection.Y) < Toolbox.Epsilon && (localOrigin.Y < -halfHeight || localOrigin.Y > halfHeight))
{
return(false);
}
inverseDirection = 1 / localDirection.Y;
t1 = (-halfHeight - localOrigin.Y) * inverseDirection;
t2 = (halfHeight - localOrigin.Y) * inverseDirection;
tempNormal = new Vector3(0, -1, 0);
if (t1 > t2)
{
temp = t1;
t1 = t2;
t2 = temp;
tempNormal *= -1;
}
temp = tmin;
tmin = Math.Max(tmin, t1);
if (temp != tmin)
{
normal = tempNormal;
}
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
if (Math.Abs(localDirection.Z) < Toolbox.Epsilon && (localOrigin.Z < -halfLength || localOrigin.Z > halfLength))
{
return(false);
}
inverseDirection = 1 / localDirection.Z;
t1 = (-halfLength - localOrigin.Z) * inverseDirection;
t2 = (halfLength - localOrigin.Z) * inverseDirection;
tempNormal = new Vector3(0, 0, -1);
if (t1 > t2)
{
temp = t1;
t1 = t2;
t2 = temp;
tempNormal *= -1;
}
temp = tmin;
tmin = Math.Max(tmin, t1);
if (temp != tmin)
{
normal = tempNormal;
}
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
hit.T = tmin;
Vector3.Multiply(ref ray.Direction, tmin, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
Quaternion.Transform(ref normal, ref transform.Orientation, out normal);
hit.Normal = normal;
return(true);
}
/// <summary>
/// Creates an affine transform from a rigid transform.
/// </summary>
/// <param name="rigid">Rigid transform to base the affine transform on.</param>
/// <param name="affine">Affine transform created from the rigid transform.</param>
public static void CreateFromRigidTransform(ref RigidTransform rigid, out AffineTransform affine)
{
affine.Translation = rigid.Position;
Matrix3x3.CreateFromQuaternion(ref rigid.Orientation, out affine.LinearTransform);
}
public PointSampleGlobal(quaternion rotation, float3 translation)
{
pose = new RigidTransform(rotation, translation);
}
///<summary>
/// Concatenates a rigid transform with another rigid transform's inverse.
///</summary>
///<param name="a">The first rigid transform.</param>
///<param name="b">The second rigid transform whose inverse will be concatenated to the first.</param>
///<param name="combinedTransform">Combined rigid transform.</param>
public static void MultiplyByInverse(ref RigidTransform a, ref RigidTransform b, out RigidTransform combinedTransform)
{
Invert(ref b, out combinedTransform);
Multiply(ref a, ref combinedTransform, out combinedTransform);
}
public RigidTransform GetInverse()
{
var t = new RigidTransform(pos, rot);
t.Inverse();
return t;
}
///<summary>
/// Tests if a box and sphere are colliding.
///</summary>
///<param name="box">Box to test.</param>
///<param name="sphere">Sphere to test.</param>
///<param name="boxTransform">Transform to apply to the box.</param>
///<param name="spherePosition">Transform to apply to the sphere.</param>
///<param name="contact">Contact point between the shapes, if any.</param>
///<returns>Whether or not the shapes were colliding.</returns>
public static bool AreShapesColliding(BoxShape box, SphereShape sphere, ref RigidTransform boxTransform, ref Vector3 spherePosition, out ContactData contact)
{
contact = new ContactData();
Vector3 localPosition;
RigidTransform.TransformByInverse(ref spherePosition, ref boxTransform, out localPosition);
#if !WINDOWS
Vector3 localClosestPoint = new Vector3();
#else
Vector3 localClosestPoint;
#endif
localClosestPoint.X = MathHelper.Clamp(localPosition.X, -box.halfWidth, box.halfWidth);
localClosestPoint.Y = MathHelper.Clamp(localPosition.Y, -box.halfHeight, box.halfHeight);
localClosestPoint.Z = MathHelper.Clamp(localPosition.Z, -box.halfLength, box.halfLength);
RigidTransform.Transform(ref localClosestPoint, ref boxTransform, out contact.Position);
Vector3 offset;
Vector3.Subtract(ref spherePosition, ref contact.Position, out offset);
float offsetLength = offset.LengthSquared();
if (offsetLength > (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance) * (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance))
{
return(false);
}
//Colliding.
if (offsetLength > Toolbox.Epsilon)
{
offsetLength = (float)Math.Sqrt(offsetLength);
//Outside of the box.
Vector3.Divide(ref offset, offsetLength, out contact.Normal);
contact.PenetrationDepth = sphere.collisionMargin - offsetLength;
}
else
{
//Inside of the box.
Vector3 penetrationDepths;
penetrationDepths.X = localClosestPoint.X < 0 ? localClosestPoint.X + box.halfWidth : box.halfWidth - localClosestPoint.X;
penetrationDepths.Y = localClosestPoint.Y < 0 ? localClosestPoint.Y + box.halfHeight : box.halfHeight - localClosestPoint.Y;
penetrationDepths.Z = localClosestPoint.Z < 0 ? localClosestPoint.Z + box.halfLength : box.halfLength - localClosestPoint.Z;
if (penetrationDepths.X < penetrationDepths.Y && penetrationDepths.X < penetrationDepths.Z)
{
contact.Normal = localClosestPoint.X > 0 ? Toolbox.RightVector : Toolbox.LeftVector;
contact.PenetrationDepth = penetrationDepths.X;
}
else if (penetrationDepths.Y < penetrationDepths.Z)
{
contact.Normal = localClosestPoint.Y > 0 ? Toolbox.UpVector : Toolbox.DownVector;
contact.PenetrationDepth = penetrationDepths.Y;
}
else
{
contact.Normal = localClosestPoint.Z > 0 ? Toolbox.BackVector : Toolbox.ForwardVector;
contact.PenetrationDepth = penetrationDepths.X;
}
contact.PenetrationDepth += sphere.collisionMargin;
Vector3.Transform(ref contact.Normal, ref boxTransform.Orientation, out contact.Normal);
}
return(true);
}
///<summary>
/// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA, without a margin.
///</summary>
///<param name="shapeA">First shape.</param>
///<param name="shapeB">Second shape.</param>
///<param name="direction">Extreme point direction in local space.</param>
///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
///<param name="extremePoint">The extreme point in the local space of A.</param>
public static void GetLocalMinkowskiExtremePointWithoutMargin(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePoint)
{
//Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
Vector3 extremePointB;
Vector3 negativeN;
Vector3.Negate(ref direction, out negativeN);
shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out extremePointB);
Vector3.Subtract(ref extremePoint, ref extremePointB, out extremePoint);
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
if (Shape.solidity == MobileMeshSolidity.Solid)
{
//If the convex cast is inside the mesh and the mesh is solid, it should return t = 0.
var ray = new Ray()
{
Position = startingTransform.Position, Direction = Toolbox.UpVector
};
if (Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
hit = new RayHit()
{
Location = startingTransform.Position, Normal = new Vector3(), T = 0
};
return(true);
}
}
hit = new RayHit();
BoundingBox boundingBox;
var transform = new AffineTransform {
Translation = worldTransform.Position
};
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform);
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox);
var tri = PhysicsThreadResources.GetTriangle();
var hitElements = CommonResources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref transform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref transform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref transform, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.MaximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
radius = tri.vC.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
tri.MaximumRadius = (float)Math.Sqrt(tri.MaximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform {
Orientation = Quaternion.Identity, Position = center
};
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.MaximumRadius = 0;
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(hit.T != float.MaxValue);
}
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(false);
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
hit = new RayHit();
BoundingBox boundingBox;
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out boundingBox);
var tri = Resources.GetTriangle();
var hitElements = Resources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref worldTransform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref worldTransform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref worldTransform, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.maximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.maximumRadius < radius)
{
tri.maximumRadius = radius;
}
radius = tri.vC.LengthSquared();
if (tri.maximumRadius < radius)
{
tri.maximumRadius = radius;
}
tri.maximumRadius = (float)Math.Sqrt(tri.maximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform();
triangleTransform.Orientation = Quaternion.Identity;
triangleTransform.Position = center;
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.maximumRadius = 0;
Resources.GiveBack(tri);
Resources.GiveBack(hitElements);
return(hit.T != float.MaxValue);
}
Resources.GiveBack(tri);
Resources.GiveBack(hitElements);
return(false);
}
public static void AreEqual(RigidTransform a, RigidTransform b, float delta = 0.0f)
{
AreEqual(a.rot, b.rot, delta);
AreEqual(a.pos, b.pos, delta);
}
///<summary>
/// Constructs a convex shape entry with identity transformation.
///</summary>
///<param name="shape">Shape of the entry.</param>
public ConvexShapeEntry(ConvexShape shape)
{
Transform = RigidTransform.Identity;
CollisionShape = shape;
}
/// <summary>
/// Constructs a convex shape entry.
/// </summary>
/// <param name="transform">Local transform of the entry.</param>
/// <param name="shape">Shape of the entry.</param>
public ConvexShapeEntry(RigidTransform transform, ConvexShape shape)
{
Transform = transform;
CollisionShape = shape;
}
/// <summary>
/// Gets the normal of the triangle in world space.
/// </summary>
/// <param name="transform">World transform.</param>
/// <returns>Normal of the triangle in world space.</returns>
public System.Numerics.Vector3 GetNormal(RigidTransform transform)
{
System.Numerics.Vector3 normal = GetLocalNormal();
QuaternionEx.Transform(ref normal, ref transform.Orientation, out normal);
return(normal);
}
///<summary>
/// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA.
///</summary>
///<param name="shapeA">First shape.</param>
///<param name="shapeB">Second shape.</param>
///<param name="direction">Extreme point direction in local space.</param>
///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
/// <param name="extremePointA">The extreme point on shapeA.</param>
/// <param name="extremePointB">The extreme point on shapeB.</param>
///<param name="extremePoint">The extreme point in the local space of A.</param>
public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB,
out Vector3 extremePointA, out Vector3 extremePointB, out Vector3 extremePoint)
{
//Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePointA);
Vector3 v;
Vector3.Negate(ref direction, out v);
shapeB.GetExtremePointWithoutMargin(v, ref localTransformB, out extremePointB);
ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, direction, ref extremePointA, ref extremePointB);
Vector3.Subtract(ref extremePointA, ref extremePointB, out extremePoint);
}
/// <summary>
/// Inverts a rigid transform.
/// </summary>
/// <param name="transform">Transform to invert.</param>
/// <param name="inverse">Inverse of the transform.</param>
public static void Invert(ref RigidTransform transform, out RigidTransform inverse)
{
QuaternionEx.Conjugate(ref transform.Orientation, out inverse.Orientation);
QuaternionEx.Transform(ref transform.Position, ref inverse.Orientation, out inverse.Position);
Vector3Ex.Negate(ref inverse.Position, out inverse.Position);
}
public static unsafe void CollideAndIntegrate(
CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Unity.Physics.Collider *collider,
ref RigidTransform transform, ref float3 linearVelocity, ref NativeStream.Writer deferredImpulseWriter,
NativeList <StatefulCollisionEvent> collisionEvents = default, NativeList <StatefulTriggerEvent> triggerEvents = default)
{
// Copy parameters
float deltaTime = stepInput.DeltaTime;
float3 up = stepInput.Up;
PhysicsWorld world = stepInput.World;
float remainingTime = deltaTime;
float3 newPosition = transform.pos;
quaternion orientation = transform.rot;
float3 newVelocity = linearVelocity;
float maxSlopeCos = math.cos(stepInput.MaxSlope);
const float timeEpsilon = 0.000001f;
for (int i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++)
{
NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp);
// Do a collider cast
{
float3 displacement = newVelocity * remainingTime;
NativeList <ColliderCastHit> triggerHits = default;
if (triggerEvents.IsCreated)
{
triggerHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity / 4, Allocator.Temp);
}
NativeList <ColliderCastHit> castHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
CharacterControllerAllHitsCollector <ColliderCastHit> collector = new CharacterControllerAllHitsCollector <ColliderCastHit>(
stepInput.RigidBodyIndex, 1.0f, ref castHits, world, triggerHits);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = newPosition,
End = newPosition + displacement
};
world.CastCollider(input, ref collector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
{
ColliderCastHit hit = collector.AllHits[hitIndex];
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, math.dot(-hit.SurfaceNormal, hit.Fraction * displacement),
stepInput.SkinWidth, maxSlopeCos, ref constraints);
}
// Update trigger events
if (triggerEvents.IsCreated)
{
UpdateTriggersSeen(stepInput, triggerHits, triggerEvents, collector.MinHitFraction);
}
}
// Then do a collider distance for penetration recovery,
// but only fix up penetrating hits
{
// Collider distance query
NativeList <DistanceHit> distanceHits = new NativeList <DistanceHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
CharacterControllerAllHitsCollector <DistanceHit> distanceHitsCollector = new CharacterControllerAllHitsCollector <DistanceHit>(
stepInput.RigidBodyIndex, stepInput.ContactTolerance, ref distanceHits, world);
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = stepInput.ContactTolerance,
Transform = transform,
Collider = collider
};
world.CalculateDistance(input, ref distanceHitsCollector);
}
// Iterate over penetrating hits and fix up distance and normal
int numConstraints = constraints.Length;
for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
if (hit.Distance < stepInput.SkinWidth)
{
bool found = false;
// Iterate backwards to locate the original constraint before the max slope constraint
for (int constraintIndex = numConstraints - 1; constraintIndex >= 0; constraintIndex--)
{
SurfaceConstraintInfo constraint = constraints[constraintIndex];
if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
constraint.ColliderKey.Equals(hit.ColliderKey))
{
// Fix up the constraint (normal, distance)
{
// Create new constraint
CreateConstraintFromHit(world, hit.RigidBodyIndex, hit.ColliderKey,
hit.Position, hit.SurfaceNormal, hit.Distance,
stepInput.SkinWidth, out SurfaceConstraintInfo newConstraint);
// Resolve its penetration
ResolveConstraintPenetration(ref newConstraint);
// Write back
constraints[constraintIndex] = newConstraint;
}
found = true;
break;
}
}
// Add penetrating hit not caught by collider cast
if (!found)
{
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance,
stepInput.SkinWidth, maxSlopeCos, ref constraints);
}
}
}
}
// Min delta time for solver to break
float minDeltaTime = 0.0f;
if (math.lengthsq(newVelocity) > k_SimplexSolverEpsilonSq)
{
// Min delta time to travel at least 1cm
minDeltaTime = 0.01f / math.length(newVelocity);
}
// Solve
float3 prevVelocity = newVelocity;
float3 prevPosition = newPosition;
SimplexSolver.Solve(remainingTime, minDeltaTime, up, stepInput.MaxMovementSpeed, constraints, ref newPosition, ref newVelocity, out float integratedTime);
// Apply impulses to hit bodies and store collision events
if (affectBodies || collisionEvents.IsCreated)
{
CalculateAndStoreDeferredImpulsesAndCollisionEvents(stepInput, affectBodies, characterMass,
prevVelocity, constraints, ref deferredImpulseWriter, collisionEvents);
}
// Calculate new displacement
float3 newDisplacement = newPosition - prevPosition;
// If simplex solver moved the character we need to re-cast to make sure it can move to new position
if (math.lengthsq(newDisplacement) > k_SimplexSolverEpsilon)
{
// Check if we can walk to the position simplex solver has suggested
var newCollector = new CharacterControllerClosestHitCollector <ColliderCastHit>(constraints, world, stepInput.RigidBodyIndex, 1.0f);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = prevPosition,
End = prevPosition + newDisplacement
};
world.CastCollider(input, ref newCollector);
if (newCollector.NumHits > 0)
{
ColliderCastHit hit = newCollector.ClosestHit;
// Move character along the newDisplacement direction until it reaches this new contact
{
Assert.IsTrue(hit.Fraction >= 0.0f && hit.Fraction <= 1.0f);
integratedTime *= hit.Fraction;
newPosition = prevPosition + newDisplacement * hit.Fraction;
}
}
}
// Reduce remaining time
remainingTime -= integratedTime;
// Write back position so that the distance query will update results
transform.pos = newPosition;
}
// Write back final velocity
linearVelocity = newVelocity;
}
///<summary>
/// Transforms a position by a rigid transform's inverse.
///</summary>
///<param name="position">Position to transform.</param>
///<param name="transform">Transform to invert and apply.</param>
///<param name="result">Transformed position.</param>
public static void TransformByInverse(ref System.Numerics.Vector3 position, ref RigidTransform transform, out System.Numerics.Vector3 result)
{
System.Numerics.Quaternion orientation;
System.Numerics.Vector3 intermediate;
Vector3Ex.Subtract(ref position, ref transform.Position, out intermediate);
QuaternionEx.Conjugate(ref transform.Orientation, out orientation);
QuaternionEx.Transform(ref intermediate, ref orientation, out result);
}
public void Interpolate(RigidTransform to, float t)
{
pos = Lerp(pos, to.pos, t);
rot = Slerp(rot, to.rot, t);
}
public void Multiply(RigidTransform a, RigidTransform b)
{
rot = a.rot * b.rot;
pos = a.pos + a.rot * b.pos;
}
public override void UpdateCollision(float dt)
{
WasContaining = Containing;
WasTouching = Touching;
mobileTriangle.collisionMargin = mesh.Shape.MeshCollisionMargin;
//Scan the pairs in sequence, updating the state as we go.
//Touching can be set to true by a single touching subpair.
Touching = false;
//Containing can be set to false by a single noncontaining or nontouching subpair.
Containing = true;
var meshData = mesh.Shape.TriangleMesh.Data;
RigidTransform mobileTriangleTransform, detectorTriangleTransform;
mobileTriangleTransform.Orientation = Quaternion.Identity;
detectorTriangleTransform.Orientation = Quaternion.Identity;
for (int i = 0; i < meshData.Indices.Length; i += 3)
{
//Grab a triangle associated with the mobile mesh.
meshData.GetTriangle(i, out mobileTriangle.vA, out mobileTriangle.vB, out mobileTriangle.vC);
RigidTransform.Transform(ref mobileTriangle.vA, ref mesh.worldTransform, out mobileTriangle.vA);
RigidTransform.Transform(ref mobileTriangle.vB, ref mesh.worldTransform, out mobileTriangle.vB);
RigidTransform.Transform(ref mobileTriangle.vC, ref mesh.worldTransform, out mobileTriangle.vC);
Vector3.Add(ref mobileTriangle.vA, ref mobileTriangle.vB, out mobileTriangleTransform.Position);
Vector3.Add(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangleTransform.Position);
Vector3.Multiply(ref mobileTriangleTransform.Position, 1 / 3f, out mobileTriangleTransform.Position);
Vector3.Subtract(ref mobileTriangle.vA, ref mobileTriangleTransform.Position, out mobileTriangle.vA);
Vector3.Subtract(ref mobileTriangle.vB, ref mobileTriangleTransform.Position, out mobileTriangle.vB);
Vector3.Subtract(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangle.vC);
//Go through all the detector volume triangles which are near the mobile mesh triangle.
bool triangleTouching, triangleContaining;
BoundingBox mobileBoundingBox;
mobileTriangle.GetBoundingBox(ref mobileTriangleTransform, out mobileBoundingBox);
DetectorVolume.TriangleMesh.Tree.GetOverlaps(mobileBoundingBox, overlaps);
for (int j = 0; j < overlaps.Count; j++)
{
DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[j], out detectorTriangle.vA, out detectorTriangle.vB, out detectorTriangle.vC);
Vector3.Add(ref detectorTriangle.vA, ref detectorTriangle.vB, out detectorTriangleTransform.Position);
Vector3.Add(ref detectorTriangle.vC, ref detectorTriangleTransform.Position, out detectorTriangleTransform.Position);
Vector3.Multiply(ref detectorTriangleTransform.Position, 1 / 3f, out detectorTriangleTransform.Position);
Vector3.Subtract(ref detectorTriangle.vA, ref detectorTriangleTransform.Position, out detectorTriangle.vA);
Vector3.Subtract(ref detectorTriangle.vB, ref detectorTriangleTransform.Position, out detectorTriangle.vB);
Vector3.Subtract(ref detectorTriangle.vC, ref detectorTriangleTransform.Position, out detectorTriangle.vC);
//If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.)))
//[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker than GJK when objects are overlapping. The GJK implementation does better on separated objects.]
if (MPRToolbox.AreShapesOverlapping(detectorTriangle, mobileTriangle, ref detectorTriangleTransform, ref mobileTriangleTransform))
{
triangleTouching = true;
//The convex can't be fully contained if it's still touching the surface.
triangleContaining = false;
overlaps.Clear();
goto finishTriangleTest;
}
}
overlaps.Clear();
//If we get here, then there was no shell intersection.
//If the convex's center point is contained by the mesh, then the convex is fully contained.
//This test is only needed if containment hasn't yet been outlawed or a touching state hasn't been established.
if ((!Touching || Containing) && DetectorVolume.IsPointContained(ref mobileTriangleTransform.Position, overlaps))
{
triangleTouching = true;
triangleContaining = true;
goto finishTriangleTest;
}
//If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate!
triangleTouching = false;
triangleContaining = false;
finishTriangleTest:
//Analyze the results of the triangle test.
if (triangleTouching)
{
Touching = true; //If one child is touching, then we are touching too.
}
else
{
Containing = false; //If one child isn't touching, then we aren't containing.
}
if (!triangleContaining) //If one child isn't containing, then we aren't containing.
{
Containing = false;
}
if (!Containing && Touching)
{
//If it's touching but not containing, no further pairs will change the state.
//Containment has been invalidated by something that either didn't touch or wasn't contained.
//Touching has been ensured by at least one object touching.
break;
}
}
//There is a possibility that the MobileMesh is solid and fully contains the DetectorVolume.
//In this case, we should be Touching, but currently we are not.
if (mesh.Shape.solidity == MobileMeshSolidity.Solid && !Containing && !Touching)
{
//To determine if the detector volume is fully contained, check if one of the detector mesh's vertices
//are in the mobile mesh.
//This *could* fail if the mobile mesh is actually multiple pieces, but that's not a common or really supported case for solids.
Vector3 vertex;
DetectorVolume.TriangleMesh.Data.GetVertexPosition(0, out vertex);
Ray ray;
ray.Direction = Vector3.Up;
RayHit hit;
RigidTransform.TransformByInverse(ref vertex, ref mesh.worldTransform, out ray.Position);
if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
Touching = true;
}
}
NotifyDetectorVolumeOfChanges();
}
///<summary>
/// Transforms a rigid transform by another rigid transform's inverse.
///</summary>
///<param name="a">The first rigid transform.</param>
///<param name="b">The second rigid transform, to be inverted.</param>
///<param name="combinedTransform">Combined rigid transform.</param>
public static void TransformByInverse(ref RigidTransform a, ref RigidTransform b, out RigidTransform combinedTransform)
{
Invert(ref b, out combinedTransform);
Transform(ref a, ref combinedTransform, out combinedTransform);
}
///<summary>
/// Constructs a new compound shape entry using the volume of the shape as a weight.
///</summary>
///<param name="shape">Shape to use.</param>
public CompoundShapeEntry(EntityShape shape)
{
LocalTransform = RigidTransform.Identity;
Shape = shape;
Weight = shape.Volume;
}
public void Interpolate(RigidTransform to, float t)
{
pos = SteamVR_Utils.Lerp(pos, to.pos, t);
rot = SteamVR_Utils.Slerp(rot, to.rot, t);
}
///<summary>
/// Constructs a new compound shape entry using the volume of the shape as a weight.
///</summary>
///<param name="shape">Shape to use.</param>
///<param name="weight">Weight of the entry. This defines how much the entry contributes to its owner
/// for the purposes of center of rotation computation.</param>
public CompoundShapeEntry(EntityShape shape, float weight)
{
LocalTransform = RigidTransform.Identity;
Shape = shape;
Weight = weight;
}
public static RigidTransform Interpolate(RigidTransform a, RigidTransform b, float t)
{
return(new RigidTransform(Vector3.Lerp(a.pos, b.pos, t), Quaternion.Slerp(a.rot, b.rot, t)));
}
/// <summary> /// Sweeps a convex shape against the entry. /// </summary> /// <param name="castShape">Swept shape.</param> /// <param name="startingTransform">Beginning location and orientation of the cast shape.</param> /// <param name="sweep">Sweep motion to apply to the cast shape.</param> /// <param name="hit">Hit data of the cast on the entry, if any.</param> /// <returns>Whether or not the cast hit the entry.</returns> public abstract bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit);
public static void DrawDebugHull(NativeHull hull, RigidTransform t, DebugHullFlags options = DebugHullFlags.All, Color BaseColor = default)
{
if (options == DebugHullFlags.None)
{
return;
}
if (BaseColor == default)
{
BaseColor = Color.yellow;
}
float faceExplosionDistance = (options & DebugHullFlags.ExplodeFaces) != 0 ? 0.3f : 0;
// Iterate each twin pair at the same time.
for (int j = 0; j < hull.edgeCount; j = j + 2)
{
var edge = hull.GetEdge(j);
var twin = hull.GetEdge(j + 1);
var edgePlane = edge.face != -1 ? hull.GetPlane(edge.face) : new NativePlane();
var twinPlane = twin.face != -1 ? hull.GetPlane(twin.face) : new NativePlane();
var rotatedEdgeNormal = math.rotate(t, edgePlane.Normal);
var rotatedTwinNormal = math.rotate(t, twinPlane.Normal);
var edgeVertex1 = math.transform(t, hull.GetVertex(edge.origin));
var twinVertex1 = math.transform(t, hull.GetVertex(twin.origin));
var edgeVertex2 = math.transform(t, hull.GetVertex(edge.origin));
var twinVertex2 = math.transform(t, hull.GetVertex(twin.origin));
if ((options & DebugHullFlags.Outline) != 0)
{
Debug.DrawLine(edgeVertex1 + rotatedEdgeNormal * faceExplosionDistance, twinVertex1 + rotatedEdgeNormal * faceExplosionDistance, BaseColor);
Debug.DrawLine(edgeVertex2 + rotatedTwinNormal * faceExplosionDistance, twinVertex2 + rotatedTwinNormal * faceExplosionDistance, BaseColor);
}
if ((options & DebugHullFlags.EdgeLinks) != 0)
{
Debug.DrawLine((edgeVertex1 + twinVertex1) / 2 + rotatedEdgeNormal * faceExplosionDistance, (edgeVertex2 + twinVertex2) / 2 + rotatedTwinNormal * faceExplosionDistance, Color.gray);
}
}
if ((options & DebugHullFlags.PlaneNormals) != 0)
{
hull.IterateFaces((int index, ref NativePlane plane, ref NativeHalfEdge firstEdge) =>
{
var tPlane = plane.Transform(t);
DebugDrawer.DebugArrow(tPlane.Position, tPlane.Rotation * 0.2f, BaseColor);
});
}
if ((options & DebugHullFlags.Indices) != 0)
{
var dupeCheck = new HashSet <Vector3>();
for (int i = 0; i < hull.vertexCount; i++)
{
// Offset the label if multiple verts are on the same position.
var v = math.transform(t, hull.GetVertex(i));
var offset = dupeCheck.Contains(v) ? (float3)Vector3.forward * 0.05f : 0;
DebugDrawer.DrawLabel(v + offset, i.ToString());
dupeCheck.Add(v);
}
}
if ((options & DebugHullFlags.FaceWinding) != 0)
{
for (int i = 0; i < hull.faceCount; i++)
{
var face = hull.GetFace(i);
var plane = hull.GetPlane(i);
var tPlane = t * plane;
var edge = hull.GetEdge(face.edge);
var startOrigin = edge.origin;
do
{
var nextEdge = hull.GetEdge(edge.next);
var startVert = math.transform(t, hull.GetVertex(edge.origin));
var endVert = math.transform(t, hull.GetVertex(nextEdge.origin));
var center = (endVert + startVert) / 2;
var dir = math.normalize(endVert - startVert);
var insetDir = math.normalize(math.cross(tPlane.Normal, dir));
if ((options & DebugHullFlags.ExplodeFaces) != 0)
{
DebugDrawer.DebugArrow(center + tPlane.Normal * faceExplosionDistance, dir * 0.2f, Color.black);
}
else
{
DebugDrawer.DebugArrow(center + tPlane.Normal * faceExplosionDistance + insetDir * 0.1f, dir * 0.2f, Color.black);
}
edge = nextEdge;
} while (edge.origin != startOrigin);
}
}
}
public static unsafe void CheckSupport(PhysicsWorld world, float deltaTime, RigidTransform transform,
float3 downwardsDirection, float maxSlope, float contactTolerance, Collider *collider, ref NativeArray <SurfaceConstraintInfo> constraints,
ref NativeArray <DistanceHit> checkSupportHits, out CharacterSupportState characterState)
{
// Downwards direction must be normalized
Assert.IsTrue(Math.IsNormalized(downwardsDirection));
// "Broad phase"
MaxHitsCollector <DistanceHit> collector = new MaxHitsCollector <DistanceHit>(contactTolerance, ref checkSupportHits);
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = contactTolerance,
Transform = transform,
Collider = collider
};
world.CalculateDistance(input, ref collector);
}
// Iterate over hits and create constraints from them
for (int i = 0; i < collector.NumHits; i++)
{
DistanceHit hit = collector.AllHits[i];
CreateConstraintFromHit(world, float3.zero, deltaTime, hit.RigidBodyIndex, hit.ColliderKey,
hit.Position, hit.SurfaceNormal, hit.Distance, true, out SurfaceConstraintInfo constraint);
constraints[i] = constraint;
}
// Solve downwards
float3 outVelocity = downwardsDirection;
float3 outPosition = transform.pos;
SimplexSolver.Solve(world, deltaTime, -downwardsDirection, collector.NumHits, ref constraints, ref outPosition, ref outVelocity, out float integratedTime);
// If no hits, proclaim unsupported state
if (collector.NumHits == 0)
{
characterState = CharacterSupportState.Unsupported;
}
else
{
if (math.lengthsq(downwardsDirection - outVelocity) < SimplexSolver.c_SimplexSolverEpsilon)
{
// If velocity hasn't changed significantly, declare unsupported state
characterState = CharacterSupportState.Unsupported;
}
else if (math.lengthsq(outVelocity) < SimplexSolver.c_SimplexSolverEpsilon)
{
// If velocity is very small, declare supported state
characterState = CharacterSupportState.Supported;
}
else
{
// Check if sliding or supported
outVelocity = math.normalize(outVelocity);
float slopeAngleSin = math.dot(outVelocity, downwardsDirection);
float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
float maxSlopeCosine = math.cos(maxSlope);
if (slopeAngleCosSq < maxSlopeCosine * maxSlopeCosine)
{
characterState = CharacterSupportState.Sliding;
}
else
{
characterState = CharacterSupportState.Supported;
}
}
}
}
public bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweepnorm, double slen, MaterialSolidity solidness, out RayHit hit)
{
RigidTransform rot = new RigidTransform(Vector3.Zero, startingTransform.Orientation);
castShape.GetBoundingBox(ref rot, out BoundingBox bb);
double adv = 0.1f;
double max = slen + adv;
bool gotOne = false;
RayHit BestRH = default(RayHit);
Vector3 sweep = sweepnorm * slen;
for (double f = 0; f < max; f += adv)
{
Vector3 c = startingTransform.Position + sweepnorm * f;
int mx = (int)Math.Ceiling(c.X + bb.Max.X);
for (int x = (int)Math.Floor(c.X + bb.Min.X); x <= mx; x++)
{
if (x < 0 || x >= CHUNK_SIZE)
{
continue;
}
int my = (int)Math.Ceiling(c.Y + bb.Max.Y);
for (int y = (int)Math.Floor(c.Y + bb.Min.Y); y <= my; y++)
{
if (y < 0 || y >= CHUNK_SIZE)
{
continue;
}
int mz = (int)Math.Ceiling(c.Z + bb.Max.Z);
for (int z = (int)Math.Floor(c.Z + bb.Min.Z); z <= mz; z++)
{
if (z < 0 || z >= CHUNK_SIZE)
{
continue;
}
BlockInternal bi = Blocks[BlockIndex(x, y, z)];
if (solidness.HasFlag(((Material)bi.BlockMaterial).GetSolidity()))
{
EntityShape es = BlockShapeRegistry.BSD[bi.BlockData].GetShape(bi.Damage, out Location offs, false);
if (es == null)
{
continue;
}
Vector3 adj = new Vector3(x + (double)offs.X, y + (double)offs.Y, z + (double)offs.Z);
EntityCollidable coll = es.GetCollidableInstance();
//coll.LocalPosition = adj;
RigidTransform rt = new RigidTransform(Vector3.Zero, Quaternion.Identity);
coll.LocalPosition = Vector3.Zero;
coll.WorldTransform = rt;
coll.UpdateBoundingBoxForTransform(ref rt);
RigidTransform adjusted = new RigidTransform(startingTransform.Position - adj, startingTransform.Orientation);
bool b = coll.ConvexCast(castShape, ref adjusted, ref sweep, out RayHit rhit);
if (b && (!gotOne || rhit.T * slen < BestRH.T) && rhit.T >= 0)
{
gotOne = true;
BestRH = rhit;
BestRH.Location += adj;
BestRH.T *= slen; // TODO: ???
BestRH.Normal = -BestRH.Normal; // TODO: WHY?!
}
}
}
}
}
if (gotOne)
{
hit = BestRH;
return(true);
}
}
hit = new RayHit()
{
Location = startingTransform.Position + sweep, Normal = new Vector3(0, 0, 0), T = slen
};
return(false);
}
public static unsafe void CollideAndIntegrate(PhysicsWorld world, float deltaTime,
int maxIterations, float3 up, float3 gravity,
float characterMass, float tau, float damping, bool affectBodies, Collider *collider,
ref NativeArray <DistanceHit> distanceHits, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints,
ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
{
float remainingTime = deltaTime;
float3 lastDisplacement = linearVelocity * remainingTime;
float3 newPosition = transform.pos;
quaternion orientation = transform.rot;
float3 newVelocity = linearVelocity;
const float timeEpsilon = 0.000001f;
for (int i = 0; i < maxIterations && remainingTime > timeEpsilon; i++)
{
// First do distance query for penetration recovery
MaxHitsCollector <DistanceHit> distanceHitsCollector = new MaxHitsCollector <DistanceHit>(0.0f, ref distanceHits);
int numConstraints = 0;
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = 0.0f,
Transform = new RigidTransform
{
pos = newPosition,
rot = orientation,
},
Collider = collider
};
world.CalculateDistance(input, ref distanceHitsCollector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position,
hit.SurfaceNormal, hit.Distance, false, out SurfaceConstraintInfo constraint);
constraints[numConstraints++] = constraint;
}
}
// Then do a collider cast
{
float3 displacement = lastDisplacement - up * timeEpsilon;
float3 endPosition = newPosition + displacement;
MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Position = newPosition,
Direction = displacement
};
world.CastCollider(input, ref collector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
{
ColliderCastHit hit = collector.AllHits[hitIndex];
bool found = false;
for (int distanceHitIndex = 0; distanceHitIndex < distanceHitsCollector.NumHits; distanceHitIndex++)
{
DistanceHit dHit = distanceHitsCollector.AllHits[distanceHitIndex];
if (dHit.RigidBodyIndex == hit.RigidBodyIndex &&
dHit.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
// Skip duplicate hits
if (!found)
{
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal,
hit.Fraction * math.length(lastDisplacement), false, out SurfaceConstraintInfo constraint);
constraints[numConstraints++] = constraint;
}
}
}
// petarm.todo: Add max slope plane to avoid climbing the not allowed slopes
// Solve
float3 prevVelocity = newVelocity;
SimplexSolver.Solve(world, deltaTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime);
remainingTime -= integratedTime;
lastDisplacement = newVelocity * remainingTime;
// Apply impulses to hit bodies
if (affectBodies)
{
ResolveContacts(world, deltaTime, gravity, tau, damping, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter);
}
}
// Write back position and velocity
transform.pos = newPosition;
linearVelocity = newVelocity;
}
public static unsafe void CheckSupport(
ref PhysicsWorld world, ref PhysicsCollider collider, CharacterControllerStepInput stepInput, RigidTransform transform,
out CharacterSupportState characterState, out float3 surfaceNormal, out float3 surfaceVelocity,
NativeList <StatefulCollisionEvent> collisionEvents = default)
{
surfaceNormal = float3.zero;
surfaceVelocity = float3.zero;
// Up direction must be normalized
Assert.IsTrue(Unity.Physics.Math.IsNormalized(stepInput.Up));
// Query the world
NativeList <ColliderCastHit> castHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
CharacterControllerAllHitsCollector <ColliderCastHit> castHitsCollector = new CharacterControllerAllHitsCollector <ColliderCastHit>(
stepInput.RigidBodyIndex, 1.0f, ref castHits, world);
var maxDisplacement = -stepInput.ContactTolerance * stepInput.Up;
{
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider.ColliderPtr,
Orientation = transform.rot,
Start = transform.pos,
End = transform.pos + maxDisplacement
};
world.CastCollider(input, ref castHitsCollector);
}
// If no hits, proclaim unsupported state
if (castHitsCollector.NumHits == 0)
{
characterState = CharacterSupportState.Unsupported;
return;
}
float maxSlopeCos = math.cos(stepInput.MaxSlope);
// Iterate over distance hits and create constraints from them
NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp);
float maxDisplacementLength = math.length(maxDisplacement);
for (int i = 0; i < castHitsCollector.NumHits; i++)
{
ColliderCastHit hit = castHitsCollector.AllHits[i];
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * maxDisplacementLength,
stepInput.SkinWidth, maxSlopeCos, ref constraints);
}
// Velocity for support checking
float3 initialVelocity = maxDisplacement / stepInput.DeltaTime;
Math.ClampToMaxLength(stepInput.MaxMovementSpeed, ref initialVelocity);
// Solve downwards (don't use min delta time, try to solve full step)
float3 outVelocity = initialVelocity;
float3 outPosition = transform.pos;
SimplexSolver.Solve(stepInput.DeltaTime, stepInput.DeltaTime, stepInput.Up, stepInput.MaxMovementSpeed,
constraints, ref outPosition, ref outVelocity, out float integratedTime, false);
// Get info on surface
int numSupportingPlanes = 0;
{
for (int j = 0; j < constraints.Length; j++)
{
var constraint = constraints[j];
if (constraint.Touched && !constraint.IsTooSteep && !constraint.IsMaxSlope)
{
numSupportingPlanes++;
surfaceNormal += constraint.Plane.Normal;
surfaceVelocity += constraint.Velocity;
// Add supporting planes to collision events
if (collisionEvents.IsCreated)
{
var collisionEvent = new StatefulCollisionEvent()
{
EntityA = stepInput.World.Bodies[stepInput.RigidBodyIndex].Entity,
EntityB = stepInput.World.Bodies[constraint.RigidBodyIndex].Entity,
BodyIndexA = stepInput.RigidBodyIndex,
BodyIndexB = constraint.RigidBodyIndex,
ColliderKeyA = ColliderKey.Empty,
ColliderKeyB = constraint.ColliderKey,
Normal = constraint.Plane.Normal
};
collisionEvent.CollisionDetails = new StatefulCollisionEvent.Details(1, 0, constraint.HitPosition);
collisionEvents.Add(collisionEvent);
}
}
}
if (numSupportingPlanes > 0)
{
float invNumSupportingPlanes = 1.0f / numSupportingPlanes;
surfaceNormal *= invNumSupportingPlanes;
surfaceVelocity *= invNumSupportingPlanes;
surfaceNormal = math.normalize(surfaceNormal);
}
}
// Check support state
{
if (math.lengthsq(initialVelocity - outVelocity) < k_SimplexSolverEpsilonSq)
{
// If velocity hasn't changed significantly, declare unsupported state
characterState = CharacterSupportState.Unsupported;
}
else if (math.lengthsq(outVelocity) < k_SimplexSolverEpsilonSq && numSupportingPlanes > 0)
{
// If velocity is very small, declare supported state
characterState = CharacterSupportState.Supported;
}
else
{
// Check if sliding
outVelocity = math.normalize(outVelocity);
float slopeAngleSin = math.max(0.0f, math.dot(outVelocity, -stepInput.Up));
float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
if (slopeAngleCosSq <= maxSlopeCos * maxSlopeCos)
{
characterState = CharacterSupportState.Sliding;
}
else if (numSupportingPlanes > 0)
{
characterState = CharacterSupportState.Supported;
}
else
{
// If numSupportingPlanes is 0, surface normal is invalid, so state is unsupported
characterState = CharacterSupportState.Unsupported;
}
}
}
}
/// <summary>
/// Creates an affine transform from a rigid transform.
/// </summary>
/// <param name="rigid">Rigid transform to base the affine transform on.</param>
/// <param name="affine">Affine transform created from the rigid transform.</param>
public static void CreateFromRigidTransform(ref RigidTransform rigid, out AffineTransform affine)
{
affine.Translation = rigid.Position;
Matrix3x3.CreateFromQuaternion(ref rigid.Orientation, out affine.LinearTransform);
}
///<summary>
/// Concatenates a rigid transform with another rigid transform.
///</summary>
///<param name="a">The first rigid transform.</param>
///<param name="b">The second rigid transform.</param>
///<param name="combined">Concatenated rigid transform.</param>
public static void Multiply(ref RigidTransform a, ref RigidTransform b, out RigidTransform combined)
{
System.Numerics.Vector3 intermediate;
QuaternionEx.Transform(ref a.Position, ref b.Orientation, out intermediate);
Vector3Ex.Add(ref intermediate, ref b.Position, out combined.Position);
QuaternionEx.Concatenate(ref a.Orientation, ref b.Orientation, out combined.Orientation);
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="filter">Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present
/// in the entry, this filter will be passed into inner ray casts.</param>
/// <param name="rayHit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func <BroadPhaseEntry, bool> filter, out RayHit rayHit)
{
CompoundChild hitChild;
bool hit = ConvexCast(castShape, ref startingTransform, ref sweep, filter, out rayHit, out hitChild);
return(hit);
}
///<summary>
/// Transforms a position by a rigid transform.
///</summary>
///<param name="position">Position to transform.</param>
///<param name="transform">Transform to apply.</param>
///<param name="result">Transformed position.</param>
public static void Transform(ref System.Numerics.Vector3 position, ref RigidTransform transform, out System.Numerics.Vector3 result)
{
System.Numerics.Vector3 intermediate;
QuaternionEx.Transform(ref position, ref transform.Orientation, out intermediate);
Vector3Ex.Add(ref intermediate, ref transform.Position, out result);
}
///<summary>
/// Transforms a position by a rigid transform.
///</summary>
///<param name="position">Position to transform.</param>
///<param name="transform">Transform to apply.</param>
///<param name="result">Transformed position.</param>
public static void Transform(ref Vector3 position, ref RigidTransform transform, out Vector3 result)
{
Vector3 intermediate;
Quaternion.Transform(ref position, ref transform.Orientation, out intermediate);
Vector3.Add(ref intermediate, ref transform.Position, out result);
}
public static RigidTransform Interpolate(RigidTransform a, RigidTransform b, float t)
{
return new RigidTransform(Vector3.Lerp(a.pos, b.pos, t), Quaternion.Slerp(a.rot, b.rot, t));
}
///<summary>
/// Transforms a position by a rigid transform's inverse.
///</summary>
///<param name="position">Position to transform.</param>
///<param name="transform">Transform to invert and apply.</param>
///<param name="result">Transformed position.</param>
public static void TransformByInverse(ref Vector3 position, ref RigidTransform transform, out Vector3 result)
{
Quaternion orientation;
Vector3 intermediate;
Vector3.Subtract(ref position, ref transform.Position, out intermediate);
Quaternion.Conjugate(ref transform.Orientation, out orientation);
Quaternion.Transform(ref intermediate, ref orientation, out result);
}
public void Interpolate(RigidTransform to, float t)
{
pos = SteamVR_Utils.Lerp(pos, to.pos, t);
rot = SteamVR_Utils.Slerp(rot, to.rot, t);
}
/// <summary>
/// Finds a supporting entity, the contact location, and the contact normal.
/// </summary>
/// <param name="location">Contact point between the wheel and the support.</param>
/// <param name="normal">Contact normal between the wheel and the support.</param>
/// <param name="suspensionLength">Length of the suspension at the contact.</param>
/// <param name="supportingCollidable">Collidable supporting the wheel, if any.</param>
/// <param name="entity">Supporting object.</param>
/// <param name="material">Material of the wheel.</param>
/// <returns>Whether or not any support was found.</returns>
protected internal override bool FindSupport(out Vector3 location, out Vector3 normal, out float suspensionLength, out Collidable supportingCollidable, out Entity entity, out Material material)
{
suspensionLength = float.MaxValue;
location = Toolbox.NoVector;
supportingCollidable = null;
entity = null;
normal = Toolbox.NoVector;
material = null;
Collidable testCollidable;
RayHit rayHit;
bool hit = false;
Quaternion localSteeringTransform;
Quaternion.CreateFromAxisAngle(ref wheel.suspension.localDirection, steeringAngle, out localSteeringTransform);
var startingTransform = new RigidTransform
{
Position = wheel.suspension.worldAttachmentPoint,
Orientation = Quaternion.Concatenate(Quaternion.Concatenate(LocalWheelOrientation, IncludeSteeringTransformInCast ? localSteeringTransform : Quaternion.Identity), wheel.vehicle.Body.orientation)
};
Vector3 sweep;
Vector3.Multiply(ref wheel.suspension.worldDirection, wheel.suspension.restLength, out sweep);
for (int i = 0; i < detector.CollisionInformation.pairs.Count; i++)
{
var pair = detector.CollisionInformation.pairs[i];
testCollidable = (pair.BroadPhaseOverlap.entryA == detector.CollisionInformation ? pair.BroadPhaseOverlap.entryB : pair.BroadPhaseOverlap.entryA) as Collidable;
if (testCollidable != null)
{
if (CollisionRules.CollisionRuleCalculator(this, testCollidable) == CollisionRule.Normal &&
testCollidable.ConvexCast(shape, ref startingTransform, ref sweep, out rayHit) &&
rayHit.T * wheel.suspension.restLength < suspensionLength)
{
suspensionLength = rayHit.T * wheel.suspension.restLength;
EntityCollidable entityCollidable;
if ((entityCollidable = testCollidable as EntityCollidable) != null)
{
entity = entityCollidable.Entity;
material = entityCollidable.Entity.Material;
}
else
{
entity = null;
supportingCollidable = testCollidable;
var materialOwner = testCollidable as IMaterialOwner;
if (materialOwner != null)
{
material = materialOwner.Material;
}
}
location = rayHit.Location;
normal = rayHit.Normal;
hit = true;
}
}
}
if (hit)
{
if (suspensionLength > 0)
{
float dot;
Vector3.Dot(ref normal, ref wheel.suspension.worldDirection, out dot);
if (dot > 0)
{
//The cylinder cast produced a normal which is opposite of what we expect.
Vector3.Negate(ref normal, out normal);
}
normal.Normalize();
}
else
{
Vector3.Negate(ref wheel.suspension.worldDirection, out normal);
}
return(true);
}
return(false);
}
///<summary>
/// Multiplies a rigid transform by an affine transform.
///</summary>
///<param name="a">Rigid transform.</param>
///<param name="b">Affine transform.</param>
///<param name="transform">Combined transform.</param>
public static void Multiply(ref RigidTransform a, ref AffineTransform b, out AffineTransform transform)
{
Matrix3x3 linearTransform;//Have to use temporary variable just in case b reference is transform.
Matrix3x3.CreateFromQuaternion(ref a.Orientation, out linearTransform);
Matrix3x3.Multiply(ref linearTransform, ref b.LinearTransform, out linearTransform);
Vector3 translation;
Matrix3x3.Transform(ref a.Position, ref b.LinearTransform, out translation);
Vector3.Add(ref translation, ref b.Translation, out transform.Translation);
transform.LinearTransform = linearTransform;
}
public PointSampleGlobal(RigidTransform pose)
{
this.pose = pose;
}
/// <summary>
/// Creates an affine transform from a rigid transform.
/// </summary>
/// <param name="rigid">Rigid transform to base the affine transform on.</param>
/// <returns>Affine transform created from the rigid transform.</returns>
public static AffineTransform CreateFromRigidTransform(RigidTransform rigid)
{
AffineTransform toReturn;
toReturn.Translation = rigid.Position;
Matrix3x3.CreateFromQuaternion(ref rigid.Orientation, out toReturn.LinearTransform);
return toReturn;
}
///<summary>
/// Transforms a rigid transform by another rigid transform.
///</summary>
///<param name="a">The first, "local" rigid transform.</param>
///<param name="b">The second, "world" rigid transform.</param>
///<param name="combined">Combined rigid transform.</param>
public static void Transform(ref RigidTransform a, ref RigidTransform b, out RigidTransform combined)
{
Vector3 intermediate;
Quaternion.Transform(ref a.Position, ref b.Orientation, out intermediate);
Vector3.Add(ref intermediate, ref b.Position, out combined.Position);
Quaternion.Concatenate(ref a.Orientation, ref b.Orientation, out combined.Orientation);
}
