protected override JobHandle OnUpdate(JobHandle inputDeps)
{
if (!(HasSingleton <PhysicsDebugDisplayData>() && GetSingleton <PhysicsDebugDisplayData>().DrawTriggerEvents != 0))
{
return(inputDeps);
}
unsafe
{
DebugStream.Context debugOutput = m_DebugStreamSystem.GetContext(1);
debugOutput.Begin(0);
// Allocate a block of memory to store our debug output, so it can be shared across the display/finish jobs
var sharedOutput = (DebugStream.Context *)UnsafeUtility.Malloc(sizeof(DebugStream.Context), 16, Allocator.TempJob);
sharedOutput[0] = debugOutput;
var job = new DisplayTriggerEventsJob
{
World = m_BuildPhysicsWorldSystem.PhysicsWorld,
OutputStreamContext = sharedOutput,
};
JobHandle handle = ScheduleTriggerEventsJob(job, m_StepPhysicsWorldSystem.Simulation, ref m_BuildPhysicsWorldSystem.PhysicsWorld, inputDeps);
#pragma warning disable 618
JobHandle finishHandle = new FinishDisplayTriggerEventsJob
{
OutputStreamContext = sharedOutput
}.Schedule(handle);
#pragma warning restore 618
m_EndFramePhysicsSystem.HandlesToWaitFor.Add(finishHandle);
return(handle);
}
}
public void Execute()
{
OutputStream.Begin(0);
DrawLeavesRecursive(StaticNodes, Color.yellow, 1);
DrawLeavesRecursive(DynamicNodes, Color.red, 1);
OutputStream.End();
}
public unsafe void Execute(int workItemIndex)
{
OutputStream.Begin(workItemIndex);
foreach (TriggerEvent triggerEvent in TriggerEvents)
{
RigidBody bodyA = World.Bodies[triggerEvent.BodyIndices.BodyAIndex];
RigidBody bodyB = World.Bodies[triggerEvent.BodyIndices.BodyBIndex];
bool IsTrigger(Collider *collider, ColliderKey key)
{
if (collider->CollisionType == CollisionType.Convex)
{
return(((ConvexColliderHeader *)collider)->Material.IsTrigger);
}
else
{
collider->GetLeaf(key, out ChildCollider child);
collider = child.Collider;
UnityEngine.Assertions.Assert.IsTrue(collider->CollisionType == CollisionType.Convex);
return(((ConvexColliderHeader *)collider)->Material.IsTrigger);
}
}
char[] text = "Triggered".ToCharArray();
if (IsTrigger(bodyA.Collider, triggerEvent.ColliderKeys.ColliderKeyA))
{
OutputStream.Text(text, bodyA.WorldFromBody.pos, Color.green);
}
if (IsTrigger(bodyB.Collider, triggerEvent.ColliderKeys.ColliderKeyB))
{
OutputStream.Text(text, bodyB.WorldFromBody.pos, Color.green);
}
}
OutputStream.End();
}
public void Execute()
{
OutputStream.Begin(0);
DrawLeavesRecursive(StaticNodes, Unity.DebugDisplay.ColorIndex.Yellow, 1);
DrawLeavesRecursive(DynamicNodes, Unity.DebugDisplay.ColorIndex.Red, 1);
OutputStream.End();
}
public void Execute()
{
OutputStream.Begin(0);
for (int b = 0; b < Bodies.Length; b++)
{
if (Bodies[b].Collider.IsCreated)
{
Aabb aabb = Bodies[b].Collider.Value.CalculateAabb(Bodies[b].WorldFromBody);
float3 center = aabb.Center;
OutputStream.Box(aabb.Extents, center, Quaternion.identity, DebugDisplay.ColorIndex.BrightRed);
}
}
OutputStream.End();
}
public void Execute()
{
OutputStream.Begin(0);
for (int b = 0; b < Bodies.Length; b++)
{
if (Bodies[b].Collider != null)
{
Aabb aabb = Bodies[b].Collider->CalculateAabb(Bodies[b].WorldFromBody);
float3 center = aabb.Center;
OutputStream.Box(aabb.Extents, center, Quaternion.identity, new Color(0.7f, 0.125f, 0.125f));
}
}
OutputStream.End();
}
public void Execute(int workItemIndex)
{
OutputStream.Begin(workItemIndex);
while (ManifoldIterator.HasItemsLeft())
{
ContactHeader contactHeader = ManifoldIterator.GetNextContactHeader();
for (int c = 0; c < contactHeader.NumContacts; c++)
{
Unity.Physics.ContactPoint contact = ManifoldIterator.GetNextContact();
float3 x0 = contact.Position;
float3 x1 = contactHeader.Normal * contact.Distance;
Color color = Color.green;
OutputStream.Arrow(x0, x1, color);
}
}
OutputStream.End();
}
protected override JobHandle OnUpdate(JobHandle inputDeps)
{
if (!(HasSingleton <PhysicsDebugDisplayData>() && GetSingleton <PhysicsDebugDisplayData>().DrawContacts != 0))
{
return(inputDeps);
}
SimulationCallbacks.Callback callback = (ref ISimulation simulation, ref PhysicsWorld world, JobHandle inDeps) =>
{
unsafe
{
DebugStream.Context debugOutput = m_DebugStreamSystem.GetContext(1);
debugOutput.Begin(0);
// Allocate a block of memory to store our debug output, so it can be shared across the display/finish jobs
var sharedOutput = (DebugStream.Context *)UnsafeUtility.Malloc(sizeof(DebugStream.Context), 16, Allocator.TempJob);
sharedOutput[0] = debugOutput;
var gatherJob = new DisplayContactsJob
{
DisplayContactIndices = false,
OutputStreamContext = sharedOutput
};
JobHandle gatherJobHandle = ScheduleContactsJob(gatherJob, simulation, ref world, inDeps);
var finishJob = new FinishDisplayContactsJob
{
OutputStreamContext = sharedOutput
};
return(finishJob.Schedule(gatherJobHandle));
}
};
m_StepWorld.EnqueueCallback(SimulationCallbacks.Phase.PostCreateContacts, callback);
return(inputDeps);
}
public unsafe void Execute(int workItemIndex)
{
OutputStream.Begin(workItemIndex);
foreach (CollisionEvent collisionEvent in CollisionEvents)
{
float3 offset = new float3(0, 1, 0);
RigidBody bodyA = World.Bodies[collisionEvent.BodyIndices.BodyAIndex];
RigidBody bodyB = World.Bodies[collisionEvent.BodyIndices.BodyBIndex];
float totalImpulse = math.csum(collisionEvent.AccumulatedImpulses);
bool AreCollisionEventsEnabled(Collider *collider, ColliderKey key)
{
if (collider->CollisionType == CollisionType.Convex)
{
return(((ConvexColliderHeader *)collider)->Material.EnableCollisionEvents);
}
else
{
collider->GetLeaf(key, out ChildCollider child);
collider = child.Collider;
UnityEngine.Assertions.Assert.IsTrue(collider->CollisionType == CollisionType.Convex);
return(((ConvexColliderHeader *)collider)->Material.EnableCollisionEvents);
}
}
if (AreCollisionEventsEnabled(bodyA.Collider, collisionEvent.ColliderKeys.ColliderKeyA))
{
OutputStream.Text(totalImpulse.ToString().ToCharArray(), bodyA.WorldFromBody.pos + offset, Color.blue);
}
if (AreCollisionEventsEnabled(bodyB.Collider, collisionEvent.ColliderKeys.ColliderKeyB))
{
OutputStream.Text(totalImpulse.ToString().ToCharArray(), bodyB.WorldFromBody.pos + offset, Color.blue);
}
}
OutputStream.End();
}
public void Execute()
{
OutputStream.Begin(0);
for (int m = 0; m < MotionDatas.Length; m++)
{
float3 com = MotionDatas[m].WorldFromMotion.pos;
quaternion o = MotionDatas[m].WorldFromMotion.rot;
float3 invInertiaLocal = MotionVelocities[m].InverseInertiaAndMass.xyz;
float3 il = new float3(1.0f / invInertiaLocal.x, 1.0f / invInertiaLocal.y, 1.0f / invInertiaLocal.z);
float invMass = MotionVelocities[m].InverseInertiaAndMass.w;
// Reverse the inertia tensor computation to build a box which has the inerta tensor 'il'
// The diagonal inertia of a box with dimensions h,w,d and mass m is:
// Ix = 1/12 m (ww + dd)
// Iy = 1/12 m (dd + hh)
// Iz = 1/12 m (ww + hh)
//
// For simplicity, set K = I * 12 / m
// Then K = (ww + dd, dd + hh, ww + hh)
// => ww = Kx - dd, dd = Ky - hh, hh = Kz - ww
// By manipulation:
// 2ww = Kx - Ky + Kz
// => w = ((0.5)(Kx - Ky + Kz))^-1
// Then, substitution gives h and d.
float3 k = new float3(il.x * 12 * invMass, il.y * 12 * invMass, il.z * 12 * invMass);
float w = math.sqrt((k.x - k.y + k.z) * 0.5f);
float h = math.sqrt(k.z - w * w);
float d = math.sqrt(k.y - h * h);
float3 boxSize = new float3(h, w, d);
OutputStream.Box(boxSize, com, o, Color.magenta);
}
OutputStream.End();
}
public unsafe void Execute()
{
// Color palette
Color colorA = Color.cyan;
Color colorB = Color.magenta;
Color colorError = Color.red;
Color colorRange = Color.yellow;
OutputStream.Begin(0);
for (int iJoint = 0; iJoint < Joints.Length; iJoint++)
{
Joint joint = Joints[iJoint];
JointData *jointData = joint.JointData;
RigidBody bodyA = Bodies[joint.BodyPair.BodyAIndex];
RigidBody bodyB = Bodies[joint.BodyPair.BodyBIndex];
MTransform worldFromA, worldFromB;
MTransform worldFromJointA, worldFromJointB;
{
worldFromA = new MTransform(bodyA.WorldFromBody);
worldFromB = new MTransform(bodyB.WorldFromBody);
worldFromJointA = Mul(worldFromA, jointData->AFromJoint);
worldFromJointB = Mul(worldFromB, jointData->BFromJoint);
}
float3 pivotA = worldFromJointA.Translation;
float3 pivotB = worldFromJointB.Translation;
for (int iConstraint = 0; iConstraint < jointData->NumConstraints; iConstraint++)
{
Constraint constraint = jointData->Constraints[iConstraint];
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 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:
throw new NotImplementedException();
}
// 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 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:
throw new NotImplementedException();
}
break;
default:
throw new NotImplementedException();
}
}
}
OutputStream.End();
}