public static unsafe bool ColliderCollider <T>(ColliderCastInput input, Collider *target, ref T collector) where T : struct, ICollector <ColliderCastHit>
{
if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, target->Filter))
{
return(false);
}
if (!input.QueryContext.IsInitialized)
{
input.QueryContext = QueryContext.DefaultContext;
}
switch (input.Collider->CollisionType)
{
case CollisionType.Convex:
switch (target->Type)
{
case ColliderType.Sphere:
case ColliderType.Capsule:
case ColliderType.Triangle:
case ColliderType.Quad:
case ColliderType.Box:
case ColliderType.Cylinder:
case ColliderType.Convex:
if (ConvexConvex(input, target, collector.MaxFraction, out ColliderCastHit hit))
{
return(collector.AddHit(hit));
}
return(false);
case ColliderType.Mesh:
return(ConvexMesh(input, (MeshCollider *)target, ref collector));
case ColliderType.Compound:
return(ConvexCompound(input, (CompoundCollider *)target, ref collector));
case ColliderType.Terrain:
return(ConvexTerrain(input, (TerrainCollider *)target, ref collector));
default:
SafetyChecks.ThrowNotImplementedException();
return(default);
}
case CollisionType.Composite:
case CollisionType.Terrain:
// no support for casting composite shapes
SafetyChecks.ThrowNotImplementedException();
return(default);
default:
SafetyChecks.ThrowNotImplementedException();
return(default);
}
}
public void Execute(int index)
{
SafetyChecks.IsTrue(BranchNodeOffsets[index] >= 0);
var bvh = new BoundingVolumeHierarchy(Nodes, NodeFilters);
var lastNode = bvh.BuildBranch(Points, Aabbs, Ranges[index], BranchNodeOffsets[index]);
if (NodeFilters != null)
{
bvh.BuildCombinedCollisionFilter(BodyFilters, BranchNodeOffsets[index], lastNode);
bvh.BuildCombinedCollisionFilter(Ranges[index].Root);
}
}
internal void CreateCollider(Entity rigidbodyEntity)
{
var colliderCount = RigidbodyToColliderMapping.CountValuesForKey(rigidbodyEntity);
if (colliderCount == 0)
{
return;
}
// Single collider doesn't require a compound collider.
if (colliderCount == 1)
{
var foundColliderBlob = RigidbodyToColliderMapping.TryGetFirstValue(rigidbodyEntity, out BlobAssetReference <Collider> colliderBlob, out NativeMultiHashMapIterator <Entity> ignore);
SafetyChecks.IsTrue(foundColliderBlob);
// Add the single collider to the rigidbody entity.
DstEntityManager.AddComponentData(rigidbodyEntity, new PhysicsColliderBlob {
Collider = colliderBlob
});
return;
}
// Multiple colliders required a compound collider.
var childrenArray = new NativeArray <PhysicsCompoundCollider.ColliderBlobInstance>(colliderCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var childIndex = 0;
foreach (var colliderBlob in RigidbodyToColliderMapping.GetValuesForKey(rigidbodyEntity))
{
childrenArray[childIndex++] = new PhysicsCompoundCollider.ColliderBlobInstance
{
Collider = colliderBlob,
// NOTE: Right now the relative pose of the collider with respect to the rigidbody is baked into the shape.
// Later, we'll want to remove that and only have its offset (if any) baked into it and use this transform instead.
CompoundFromChild = PhysicsTransform.Identity
};
}
// Create the compound collider.
DstEntityManager.AddComponentData(rigidbodyEntity, new PhysicsColliderBlob {
Collider = PhysicsCompoundCollider.Create(childrenArray)
});
// We've finished with the children blobs and array.
for (var i = 0; i < colliderCount; ++i)
{
childrenArray[i].Collider.Dispose();
}
childrenArray.Dispose();
}
internal bool this[int i]
{
get
{
SafetyChecks.CheckInRangeAndThrow(i, new int2(0, 7), nameof(i));
switch (i)
{
case 0: return(Tag00);
case 1: return(Tag01);
case 2: return(Tag02);
case 3: return(Tag03);
case 4: return(Tag04);
case 5: return(Tag05);
case 6: return(Tag06);
case 7: return(Tag07);
default: return(default);
}
}
set
{
SafetyChecks.CheckInRangeAndThrow(i, new int2(0, 7), nameof(i));
switch (i)
{
case 0: Tag00 = value; break;
case 1: Tag01 = value; break;
case 2: Tag02 = value; break;
case 3: Tag03 = value; break;
case 4: Tag04 = value; break;
case 5: Tag05 = value; break;
case 6: Tag06 = value; break;
case 7: Tag07 = value; break;
}
}
}
public unsafe void Polygon(ConvexHull.ConvexArray.Accessor vertices, int vertexCount, PhysicsTransform physicsTransform, Color color)
{
SafetyChecks.IsTrue(vertexCount <= PhysicsPolygonCollider.Constants.MaxVertexCount);
Writer.Write(Type.Polygon);
var polygon = new Polygon
{
Transform = physicsTransform,
VertexCount = vertexCount,
Color = color
};
UnsafeUtility.MemCpy(polygon.Vertices, vertices.GetUnsafePtr(), sizeof(float2) * vertexCount);
Writer.Write(polygon);
}
internal void SetLinks(int edge, Edge newEdge)
{
SafetyChecks.CheckIndexAndThrow(edge, 3);
switch (edge)
{
case 0:
Edge0 = newEdge;
break;
case 1:
Edge1 = newEdge;
break;
case 2:
Edge2 = newEdge;
break;
}
}
internal Edge Links(int edge)
{
SafetyChecks.CheckIndexAndThrow(edge, 3);
switch (edge)
{
case 0:
return(Edge0);
case 1:
return(Edge1);
case 2:
return(Edge2);
default:
return(default);
}
}
static unsafe bool CastCollider(
Ray ray, ref float2x2 rotation,
ref DistanceProxy proxySource, ref DistanceProxy proxyTarget,
out ColliderCastHit hit)
{
hit = default;
var transformSource = new PhysicsTransform
{
Translation = ray.Origin,
Rotation = rotation
};
// Check we're not initially overlapped.
if ((proxySource.VertexCount < 3 || proxyTarget.VertexCount < 3) &&
OverlapQueries.OverlapConvexConvex(ref transformSource, ref proxySource, ref proxyTarget))
{
return(false);
}
// B = Source
// A = Target
var radiusSource = proxySource.ConvexRadius;
var radiusTarget = proxyTarget.ConvexRadius;
var totalRadius = radiusSource + radiusTarget;
var invRotation = math.inverse(rotation);
var sweepDirection = ray.Displacement;
var normal = float2.zero;
var lambda = 0.0f;
// Initialize the simplex.
var simplex = new Simplex();
simplex.Count = 0;
var vertices = &simplex.Vertex1;
// Get a support point in the inverse direction.
var indexTarget = proxyTarget.GetSupport(-sweepDirection);
var supportTarget = proxyTarget.Vertices[indexTarget];
var indexSource = proxySource.GetSupport(PhysicsMath.mul(invRotation, sweepDirection));
var supportSource = PhysicsMath.mul(transformSource, proxySource.Vertices[indexSource]);
var v = supportTarget - supportSource;
// Sigma is the target distance between polygons
var sigma = math.max(PhysicsSettings.Constants.MinimumConvexRadius, totalRadius - PhysicsSettings.Constants.MinimumConvexRadius);
const float tolerance = PhysicsSettings.Constants.LinearSlop * 0.5f;
var iteration = 0;
while (
iteration++ < PhysicsSettings.Constants.MaxGJKInterations &&
math.abs(math.length(v) - sigma) > tolerance
)
{
if (simplex.Count >= 3)
{
SafetyChecks.ThrowInvalidOperationException("ColliderCast Simplex must have less than 3 vertex.");
}
// Support in direction -supportV (Target - Source)
indexTarget = proxyTarget.GetSupport(-v);
supportTarget = proxyTarget.Vertices[indexTarget];
indexSource = proxySource.GetSupport(PhysicsMath.mul(invRotation, v));
supportSource = PhysicsMath.mul(transformSource, proxySource.Vertices[indexSource]);
var p = supportTarget - supportSource;
v = math.normalizesafe(v);
// Intersect ray with plane.
var vp = math.dot(v, p);
var vr = math.dot(v, sweepDirection);
if (vp - sigma > lambda * vr)
{
if (vr <= 0.0f)
{
return(false);
}
lambda = (vp - sigma) / vr;
if (lambda > 1.0f)
{
return(false);
}
normal = -v;
simplex.Count = 0;
}
// Reverse simplex since it works with B - A.
// Shift by lambda * r because we want the closest point to the current clip point.
// Note that the support point p is not shifted because we want the plane equation
// to be formed in un-shifted space.
var vertex = vertices + simplex.Count;
vertex->IndexA = indexSource;
vertex->SupportA = supportSource + lambda * sweepDirection;
vertex->IndexB = indexTarget;
vertex->SupportB = supportTarget;
vertex->W = vertex->SupportB - vertex->SupportA;
vertex->A = 1.0f;
simplex.Count += 1;
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
SafetyChecks.ThrowInvalidOperationException("Simplex has invalid count.");
return(default);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
// Overlap.
return(false);
}
// Get search direction.
v = simplex.GetClosestPoint();
}
// Ensure we don't process an empty simplex.
if (simplex.Count == 0)
{
return(false);
}
// Prepare result.
var pointSource = float2.zero;
var pointTarget = float2.zero;
simplex.GetWitnessPoints(ref pointSource, ref pointTarget);
normal = math.normalizesafe(-v);
hit = new ColliderCastHit
{
Position = pointTarget + (normal * radiusTarget),
SurfaceNormal = normal,
Fraction = lambda
};
return(true);
}
public unsafe void Execute()
{
// Color palette
var colorA = Unity.DebugDisplay.ColorIndex.Cyan;
var colorB = Unity.DebugDisplay.ColorIndex.Magenta;
var colorError = Unity.DebugDisplay.ColorIndex.Red;
var colorRange = Unity.DebugDisplay.ColorIndex.Yellow;
OutputStream.Begin(0);
for (int iJoint = 0; iJoint < Joints.Length; iJoint++)
{
Joint joint = Joints[iJoint];
if (!joint.BodyPair.IsValid)
{
continue;
}
RigidBody bodyA = Bodies[joint.BodyPair.BodyIndexA];
RigidBody bodyB = Bodies[joint.BodyPair.BodyIndexB];
MTransform worldFromA, worldFromB;
MTransform worldFromJointA, worldFromJointB;
{
worldFromA = new MTransform(bodyA.WorldFromBody);
worldFromB = new MTransform(bodyB.WorldFromBody);
worldFromJointA = Mul(worldFromA, joint.AFromJoint);
worldFromJointB = Mul(worldFromB, joint.BFromJoint);
}
float3 pivotA = worldFromJointA.Translation;
float3 pivotB = worldFromJointB.Translation;
for (var i = 0; i < joint.Constraints.Length; i++)
{
Constraint constraint = joint.Constraints[i];
switch (constraint.Type)
{
case ConstraintType.Linear:
float3 diff = pivotA - pivotB;
// Draw the feature on B and find the range for A
float3 rangeOrigin;
float3 rangeDirection;
float rangeDistance;
switch (constraint.Dimension)
{
case 0:
continue;
case 1:
float3 normal = worldFromJointB.Rotation[constraint.ConstrainedAxis1D];
OutputStream.Plane(pivotB, normal * k_Scale, colorB);
rangeDistance = math.dot(normal, diff);
rangeOrigin = pivotA - normal * rangeDistance;
rangeDirection = normal;
break;
case 2:
float3 direction = worldFromJointB.Rotation[constraint.FreeAxis2D];
OutputStream.Line(pivotB - direction * k_Scale, pivotB + direction * k_Scale, colorB);
float dot = math.dot(direction, diff);
rangeOrigin = pivotB + direction * dot;
rangeDirection = diff - direction * dot;
rangeDistance = math.length(rangeDirection);
rangeDirection = math.select(rangeDirection / rangeDistance, float3.zero, rangeDistance < 1e-5);
break;
case 3:
OutputStream.Point(pivotB, k_Scale, colorB);
rangeOrigin = pivotB;
rangeDistance = math.length(diff);
rangeDirection = math.select(diff / rangeDistance, float3.zero, rangeDistance < 1e-5);
break;
default:
SafetyChecks.ThrowNotImplementedException();
return;
}
// Draw the pivot on A
OutputStream.Point(pivotA, k_Scale, colorA);
// Draw error
float3 rangeA = rangeOrigin + rangeDistance * rangeDirection;
float3 rangeMin = rangeOrigin + constraint.Min * rangeDirection;
float3 rangeMax = rangeOrigin + constraint.Max * rangeDirection;
if (rangeDistance < constraint.Min)
{
OutputStream.Line(rangeA, rangeMin, colorError);
}
else if (rangeDistance > constraint.Max)
{
OutputStream.Line(rangeA, rangeMax, colorError);
}
if (math.length(rangeA - pivotA) > 1e-5f)
{
OutputStream.Line(rangeA, pivotA, colorError);
}
// Draw the range
if (constraint.Min != constraint.Max)
{
OutputStream.Line(rangeMin, rangeMax, colorRange);
}
break;
case ConstraintType.Angular:
switch (constraint.Dimension)
{
case 0:
continue;
case 1:
// Get the limited axis and perpendicular in joint space
int constrainedAxis = constraint.ConstrainedAxis1D;
float3 axisInWorld = worldFromJointA.Rotation[constrainedAxis];
float3 perpendicularInWorld = worldFromJointA.Rotation[(constrainedAxis + 1) % 3] * k_Scale;
// Draw the angle of A
OutputStream.Line(pivotA, pivotA + perpendicularInWorld, colorA);
// Calculate the relative angle
float angle;
{
float3x3 jointBFromA = math.mul(math.inverse(worldFromJointB.Rotation), worldFromJointA.Rotation);
angle = CalculateTwistAngle(new quaternion(jointBFromA), constrainedAxis);
}
// Draw the range in B
float3 axis = worldFromJointA.Rotation[constraint.ConstrainedAxis1D];
OutputStream.Arc(pivotB, axis, math.mul(quaternion.AxisAngle(axis, constraint.Min - angle), perpendicularInWorld), constraint.Max - constraint.Min, colorB);
break;
case 2:
// Get axes in world space
int axisIndex = constraint.FreeAxis2D;
float3 axisA = worldFromJointA.Rotation[axisIndex];
float3 axisB = worldFromJointB.Rotation[axisIndex];
// Draw the cones in B
if (constraint.Min == 0.0f)
{
OutputStream.Line(pivotB, pivotB + axisB * k_Scale, colorB);
}
else
{
OutputStream.Cone(pivotB, axisB * k_Scale, constraint.Min, colorB);
}
if (constraint.Max != constraint.Min)
{
OutputStream.Cone(pivotB, axisB * k_Scale, constraint.Max, colorB);
}
// Draw the axis in A
OutputStream.Arrow(pivotA, axisA * k_Scale, colorA);
break;
case 3:
// TODO - no idea how to visualize this if the limits are nonzero :)
break;
default:
SafetyChecks.ThrowNotImplementedException();
return;
}
break;
default:
SafetyChecks.ThrowNotImplementedException();
return;
}
}
}
OutputStream.End();
}
internal bool this[int i]
{
get
{
SafetyChecks.CheckInRangeAndThrow(i, new int2(0, 31), nameof(i));
switch (i)
{
case 0: return(Category00);
case 1: return(Category01);
case 2: return(Category02);
case 3: return(Category03);
case 4: return(Category04);
case 5: return(Category05);
case 6: return(Category06);
case 7: return(Category07);
case 8: return(Category08);
case 9: return(Category09);
case 10: return(Category10);
case 11: return(Category11);
case 12: return(Category12);
case 13: return(Category13);
case 14: return(Category14);
case 15: return(Category15);
case 16: return(Category16);
case 17: return(Category17);
case 18: return(Category18);
case 19: return(Category19);
case 20: return(Category20);
case 21: return(Category21);
case 22: return(Category22);
case 23: return(Category23);
case 24: return(Category24);
case 25: return(Category25);
case 26: return(Category26);
case 27: return(Category27);
case 28: return(Category28);
case 29: return(Category29);
case 30: return(Category30);
case 31: return(Category31);
default: return(default);
}
}
set
{
SafetyChecks.CheckInRangeAndThrow(i, new int2(0, 31), nameof(i));
switch (i)
{
case 0: Category00 = value; break;
case 1: Category01 = value; break;
case 2: Category02 = value; break;
case 3: Category03 = value; break;
case 4: Category04 = value; break;
case 5: Category05 = value; break;
case 6: Category06 = value; break;
case 7: Category07 = value; break;
case 8: Category08 = value; break;
case 9: Category09 = value; break;
case 10: Category10 = value; break;
case 11: Category11 = value; break;
case 12: Category12 = value; break;
case 13: Category13 = value; break;
case 14: Category14 = value; break;
case 15: Category15 = value; break;
case 16: Category16 = value; break;
case 17: Category17 = value; break;
case 18: Category18 = value; break;
case 19: Category19 = value; break;
case 20: Category20 = value; break;
case 21: Category21 = value; break;
case 22: Category22 = value; break;
case 23: Category23 = value; break;
case 24: Category24 = value; break;
case 25: Category25 = value; break;
case 26: Category26 = value; break;
case 27: Category27 = value; break;
case 28: Category28 = value; break;
case 29: Category29 = value; break;
case 30: Category30 = value; break;
case 31: Category31 = value; break;
}
}
}
