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