private void ComputeDynamics() { this.Mass = 0.0f; this.Inertia = 0.0f; for (int i = 0; i < this.shapeCount; i++) { VoltShape shape = this.shapes[i]; if (shape.Density == 0.0f) { continue; } FP curMass = shape.Mass; FP curInertia = shape.Inertia; this.Mass += curMass; this.Inertia += curMass * curInertia; } if (this.Mass < VoltConfig.MINIMUM_DYNAMIC_MASS) { throw new InvalidOperationException("Mass of dynamic too small"); } else { this.InvMass = 1.0f / this.Mass; this.InvInertia = 1.0f / this.Inertia; } this.BodyType = VoltBodyType.Dynamic; }
private static Manifold __Polygon_Polygon( VoltWorld world, VoltShape sa, VoltShape sb) { return(Polygon_Polygon(world, (VoltPolygon)sa, (VoltPolygon)sb)); }
private static Manifold __Circle_Circle( VoltWorld world, VoltShape sa, VoltShape sb) { return(Circle_Circle(world, (VoltCircle)sa, (VoltCircle)sb)); }
private static Manifold __Polygon_Circle( VoltWorld world, VoltShape sa, VoltShape sb) { return(Circle_Polygon(world, (VoltCircle)sb, (VoltPolygon)sa)); }
/// <summary> /// Workhorse for circle-circle collisions, compares origin distance /// to the sum of the two circles' radii, returns a Manifold. /// </summary> /// private static Manifold TestCircles( VoltWorld world, VoltCircle shapeA, VoltShape shapeB, Vector2 overrideBCenter, // For testing vertices in circles float overrideBRadius) { Vector2 r = overrideBCenter - shapeA.worldSpaceOrigin; float min = shapeA.radius + overrideBRadius; float distSq = r.sqrMagnitude; if (distSq >= min * min) { return(null); } float dist = Mathf.Sqrt(distSq); float distInv = 1.0f / dist; Vector2 pos = shapeA.worldSpaceOrigin + (0.5f + distInv * (shapeA.radius - min / 2.0f)) * r; // Build the collision Manifold Manifold manifold = world.AllocateManifold().Assign(world, shapeA, shapeB); manifold.AddContact(pos, distInv * r, dist - min); return(manifold); }
/// <summary> /// Workhorse for circle-circle collisions, compares origin distance /// to the sum of the two circles' radii, returns a Manifold. /// </summary> /// private static Manifold TestCircles( VoltWorld world, VoltCircle shapeA, VoltShape shapeB, VoltVector2 overrideBCenter, // For testing vertices in circles Fix64 overrideBRadius) { VoltVector2 r = overrideBCenter - shapeA.worldSpaceOrigin; Fix64 min = shapeA.radius + overrideBRadius; Fix64 distSq = r.sqrMagnitude; if (distSq >= min * min) { return(null); } Fix64 dist = VoltMath.Sqrt(distSq); // 최소값을 지정하여 divide by zero 방지 Fix64 distInv = Fix64.One / VoltMath.Max(dist, min / (Fix64)10); VoltVector2 pos = shapeA.worldSpaceOrigin + (Fix64.One / (Fix64)2 + distInv * (shapeA.radius - min / (Fix64)2)) * r; // Build the collision Manifold Manifold manifold = world.AllocateManifold().Assign(world, shapeA, shapeB); manifold.AddContact(pos, distInv * r, dist - min); return(manifold); }
public static void Draw(VoltShape shape) { shape.GizmoDraw( new Color(1.0f, 1.0f, 0.0f, 1.0f), // Edge Color new Color(1.0f, 0.0f, 1.0f, 1.0f), // Normal Color new Color(0.0f, 0.0f, 0.0f, 1.0f), // Origin Color new Color(0.7f, 0.0f, 0.3f, 1.0f), // AABB Color 0.25f); }
internal static Manifold Dispatch( VoltWorld world, VoltShape sa, VoltShape sb) { Test test = Collision.tests[(int)sa.Type, (int)sb.Type]; return(test(world, sa, sb)); }
internal void Set( VoltShape shape, Fix64 distance, VoltVector2 normal) { if (this.IsValid == false || distance < this.distance) { this.shape = shape; this.distance = distance; this.normal = normal; } }
internal Manifold Assign( VoltWorld world, VoltShape shapeA, VoltShape shapeB) { this.world = world; this.ShapeA = shapeA; this.ShapeB = shapeB; this.Restitution = Mathf.Sqrt(shapeA.Restitution * shapeB.Restitution); this.Friction = Mathf.Sqrt(shapeA.Friction * shapeB.Friction); this.used = 0; return(this); }
internal void FreeShape(VoltShape shape) { switch (shape.Type) { case VoltShape.ShapeType.Circle: this.circlePool.Deallocate(shape); break; case VoltShape.ShapeType.Polygon: this.polygonPool.Deallocate(shape); break; default: VoltDebug.LogError("Unknown shape for deallocation"); break; } }
/// <summary> /// Creates a manifold for two shapes if they collide. /// </summary> private void NarrowPhase( VoltShape sa, VoltShape sb) { if (sa.AABB.Intersect(sb.AABB) == false) { return; } VoltShape.OrderShapes(ref sa, ref sb); Manifold manifold = Collision.Dispatch(this, sa, sb); if (manifold != null) { this.manifolds.Add(manifold); } }
/// <summary> /// Checks a ray against a circle with a given origin and square radius. /// </summary> internal static bool CircleRayCast( VoltShape shape, Vector2 shapeOrigin, float sqrRadius, ref VoltRayCast ray, ref VoltRayResult result) { Vector2 toOrigin = shapeOrigin - ray.origin; if (toOrigin.sqrMagnitude < sqrRadius) { result.SetContained(shape); return(true); } float slope = Vector2.Dot(toOrigin, ray.direction); if (slope < 0) { return(false); } float sqrSlope = slope * slope; float d = sqrRadius + sqrSlope - Vector2.Dot(toOrigin, toOrigin); if (d < 0) { return(false); } float dist = slope - Mathf.Sqrt(d); if (dist < 0 || dist > ray.distance) { return(false); } // N.B.: For historical raycasts this normal will be wrong! // Must be either transformed back to world or invalidated later. Vector2 normal = (dist * ray.direction - toOrigin).normalized; result.Set(shape, dist, normal); return(true); }
internal void SetContained(VoltShape shape) { this.shape = shape; this.distance = Fix64.Zero; this.normal = VoltVector2.zero; }
internal void Reset() { this.shape = null; }
internal void SetContained(VoltShape shape) { this.shape = shape; this.distance = 0.0f; this.normal = Vector2.zero; }