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