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