예제 #1
0
        public override bool IsColliding(PhysCollider collider, out CollisionContact manifold, out bool isBodyA)
        {
            if (collider is PhysBoxCollider2D)
            {
                PhysBoxCollider2D other = collider as PhysBoxCollider2D;

                isBodyA = false;
                return(CollisionDetector.IsCollidingRect(other.CollisionBodyWS, CollisionBodyWS, out manifold));
            }
            else if (collider is PhysSphereCollider2D)
            {
                PhysSphereCollider2D other = collider as PhysSphereCollider2D;

                isBodyA = true;
                return(CollisionDetector.IsCollidingSphere(CollisionBodyWS, other.CollisionBodyWS, out manifold));
            }
            else if (collider is PhysPolyCollider2D)
            {
                PhysPolyCollider2D other = collider as PhysPolyCollider2D;

                isBodyA = false;
                return(CollisionDetector.IsCollidingPoly(other.CollisionBodyWS, CollisionBodyWS, out manifold));
            }

            isBodyA  = false;
            manifold = null;
            return(false);
        }
예제 #2
0
        public void RegisterCollision(CollisionContact collision, bool isBodyA)
        {
            collisions.Push(new FRawCollision()
            {
                CollisionContact = collision, IsBodyA = isBodyA
            });

            if (!componentsInCollision.ContainsKey(isBodyA ? collision.B : collision.A))
            {
                componentsInCollision.TryAdd(isBodyA ? collision.B : collision.A, 0);
            }
        }
        public static bool IsCollidingSphere(FCollSphere a, FCollSphere b, out CollisionContact manifold)
        {
            bool flipRef = b.Center.x < a.Center.x || b.Center.y < a.Center.y;

            Vector2 rel = flipRef ? a.Center - b.Center : b.Center - a.Center;

            float dist = rel.sqrMagnitude;
            float rSum = a.Radius + b.Radius;

            if (dist > (rSum * rSum))
            {
                manifold = null;
                return(false);
            }

            float delta = rel.magnitude;

            manifold = new CollisionContact();

            if (delta == 0.0f)
            {
                manifold.Penetration   = a.Radius;
                manifold.Normal        = Vector2.up;
                manifold.ContactPoints = new Vector2[] { a.Center };
            }
            else
            {
                manifold.Penetration   = rSum - delta;
                manifold.Normal        = rel / delta;
                manifold.ContactPoints = new Vector2[] { flipRef?manifold.Normal *b.Radius + b.Center : manifold.Normal * a.Radius + a.Center };
            }

            manifold.BodyAInc = flipRef;

            if (flipRef)
            {
                manifold.EdgeNormalA = -manifold.Normal;
                manifold.EdgeNormalB = manifold.Normal;
            }
            else
            {
                manifold.EdgeNormalA = manifold.Normal;
                manifold.EdgeNormalB = -manifold.Normal;
            }

            return(true);
        }
        private static bool SphereSAT(FCollPoly p1, FCollSphere p2, out CollisionContact manifold)
        {
            FSATAxis[] axisPoly      = GetAxis_sat(ref p1.Vertices);
            FSATAxis   circleAxis    = GetCircleAxis_sat(p2.Center, ref p1.Vertices);
            FSATAxis   collisionAxis = axisPoly[0];
            float      penetration   = 100000f;

            manifold = null;

            for (int i = 0; i < axisPoly.Length; i++)
            {
                Vector2 proj1        = ProjOntoAxis_sat(axisPoly[i], ref p1.Vertices);
                float   circleCenter = Vector2.Dot(p2.Center, axisPoly[i].Axis);
                Vector2 proj2        = new Vector2(circleCenter - p2.Radius, circleCenter + p2.Radius);

                float overlap = 0f;

                if (!GetOverlapFromProjection_sat(proj1, proj2, out axisPoly[i].BodyA, out overlap))
                {
                    return(false);
                }
                else
                {
                    if (ContainsOtherProjection_sat(proj1, proj2) || ContainsOtherProjection_sat(proj2, proj1))
                    {
                        float mins = Mathf.Abs(proj1.x - proj2.x);
                        float maxs = Mathf.Abs(proj1.y - proj2.y);

                        if (mins < maxs)
                        {
                            overlap += mins;
                        }
                        else
                        {
                            overlap += maxs;
                        }
                    }

                    if (overlap < penetration)
                    {
                        collisionAxis = axisPoly[i];
                        penetration   = overlap;
                    }
                }
            }


            //Circle axis
            {
                Vector2 proj1        = ProjOntoAxis_sat(circleAxis, ref p1.Vertices);
                float   circleCenter = Vector2.Dot(p2.Center, circleAxis.Axis);
                Vector2 proj2        = new Vector2(circleCenter - p2.Radius, circleCenter + p2.Radius);

                float overlap = 0f;

                if (!GetOverlapFromProjection_sat(proj1, proj2, out circleAxis.BodyA, out overlap))
                {
                    return(false);
                }
                else
                {
                    if (ContainsOtherProjection_sat(proj1, proj2) || ContainsOtherProjection_sat(proj2, proj1))
                    {
                        float mins = Mathf.Abs(proj1.x - proj2.x);
                        float maxs = Mathf.Abs(proj1.y - proj2.y);

                        if (mins < maxs)
                        {
                            overlap += mins;
                        }
                        else
                        {
                            overlap += maxs;
                        }
                    }

                    if (overlap < penetration)
                    {
                        collisionAxis = circleAxis;
                        penetration   = overlap;
                    }
                }
            }

            //Debug.Log("Collision axis: " + collisionAxis.Axis);
            //Debug.Log("Penetration: " + penetration);

            manifold = new CollisionContact();

            FEdge eRef;

            if (!collisionAxis.BodyA)
            {
                collisionAxis.Axis *= -1.0f;
            }

            eRef = GetBestEdge_sat(ref p1.Vertices, collisionAxis.Axis);

            eRef.Normal = MeshUtils.CalcNormal(eRef.AV, eRef.BV);

            Vector2 vLeft     = p1.Vertices[CollMeshUtils.LeftIndex(eRef.AI, p1.Vertices.Length)];
            Vector2 vRight    = p1.Vertices[CollMeshUtils.RightIndex(eRef.BI, p1.Vertices.Length)];
            Vector2 normLeft  = MeshUtils.CalcNormalRaw(vLeft, eRef.AV);
            Vector2 normRight = MeshUtils.CalcNormalRaw(eRef.BV, vRight);
            Vector2 leftRel   = p2.Center - eRef.AV;
            Vector2 rightRel  = p2.Center - eRef.BV;

            if (Vector2.Dot(normLeft, leftRel) > 0f && Vector2.Dot(eRef.Normal, leftRel) > 0f)
            {
                manifold.Penetration   = penetration;
                manifold.Normal        = collisionAxis.Axis;
                manifold.ContactPoints = new Vector2[] { p2.Center - leftRel.normalized * p2.Radius };
            }
            else if (Vector2.Dot(normRight, rightRel) > 0f && Vector2.Dot(eRef.Normal, rightRel) > 0f)
            {
                manifold.Penetration   = penetration;
                manifold.Normal        = collisionAxis.Axis;
                manifold.ContactPoints = new Vector2[] { p2.Center - rightRel.normalized * p2.Radius };
            }
            else
            {
                manifold.Penetration   = penetration;
                manifold.Normal        = collisionAxis.Axis;
                manifold.ContactPoints = new Vector2[] { p2.Center - eRef.Normal * p2.Radius };
            }

            manifold.BodyAInc    = false;
            manifold.EdgeNormalA = eRef.Normal;
            manifold.EdgeNormalB = (p2.Center - manifold.ContactPoints[0]).normalized;

            return(true);
        }
 public static bool IsCollidingPoly(FCollPoly a, FCollSphere b, out CollisionContact manifold)
 {
     return(SphereSAT(a, b, out manifold));
 }
 public static bool IsCollidingRect(FCollPoly a, FCollPoly b, out CollisionContact manifold)
 {
     return(PolySAT(a, b, out manifold));
 }
        private static bool PolySAT(FCollPoly p1, FCollPoly p2, out CollisionContact manifold)
        {
            manifold = null;

            FSATAxis[] axis1 = GetAxis_sat(ref p1.Vertices);
            FSATAxis[] axis2 = GetAxis_sat(ref p2.Vertices);

            FSATAxis collisionAxis = axis1[0];
            float    penetration   = 100000f;

            for (int i = 0; i < axis1.Length; i++)
            {
                Vector2 proj1 = ProjOntoAxis_sat(axis1[i], ref p1.Vertices);
                Vector2 proj2 = ProjOntoAxis_sat(axis1[i], ref p2.Vertices);

                float overlap = 0f;

                if (!GetOverlapFromProjection_sat(proj1, proj2, out axis1[i].BodyA, out overlap))
                {
                    return(false);
                }
                else
                {
                    if (ContainsOtherProjection_sat(proj1, proj2) || ContainsOtherProjection_sat(proj2, proj1))
                    {
                        float mins = Mathf.Abs(proj1.x - proj2.x);
                        float maxs = Mathf.Abs(proj1.y - proj2.y);

                        if (mins < maxs)
                        {
                            overlap += mins;
                        }
                        else
                        {
                            overlap += maxs;
                        }
                    }

                    if (overlap < penetration)
                    {
                        collisionAxis = axis1[i];
                        penetration   = overlap;
                    }
                }
            }

            for (int i = 0; i < axis2.Length; i++)
            {
                Vector2 proj1 = ProjOntoAxis_sat(axis2[i], ref p1.Vertices);
                Vector2 proj2 = ProjOntoAxis_sat(axis2[i], ref p2.Vertices);

                float overlap = 0f;

                if (!GetOverlapFromProjection_sat(proj1, proj2, out axis2[i].BodyA, out overlap))
                {
                    return(false);
                }
                else
                {
                    if (ContainsOtherProjection_sat(proj1, proj2) || ContainsOtherProjection_sat(proj2, proj1))
                    {
                        float mins = Mathf.Abs(proj1.x - proj2.x);
                        float maxs = Mathf.Abs(proj1.y - proj2.y);

                        if (mins < maxs)
                        {
                            overlap += mins;
                        }
                        else
                        {
                            overlap += maxs;
                        }
                    }

                    if (overlap < penetration)
                    {
                        collisionAxis = axis2[i];
                        penetration   = overlap;
                    }
                }
            }

            manifold = new CollisionContact();

            FEdge eRef;
            FEdge eInc;

            if (collisionAxis.BodyA)
            {
                eRef = GetBestEdge_sat(ref p1.Vertices, collisionAxis.Axis);
                eInc = GetBestEdge_sat(ref p2.Vertices, -collisionAxis.Axis);
            }
            else
            {
                eInc = GetBestEdge_sat(ref p1.Vertices, -collisionAxis.Axis);
                eRef = GetBestEdge_sat(ref p2.Vertices, collisionAxis.Axis);
            }

            bool flipped = false;

            if (Mathf.Abs(Vector2.Dot(eInc.BV - eInc.AV, collisionAxis.Axis)) < Mathf.Abs(Vector2.Dot(eRef.BV - eRef.AV, collisionAxis.Axis)))
            {
                FEdge copy = eRef;
                eRef = eInc;
                eInc = copy;

                flipped = true;
            }

            Vector2 refDir = (eRef.BV - eRef.AV).normalized;

            float offset = Vector2.Dot(refDir, eRef.AV);

            List <Vector2> contactPoints = Clip_sat(eInc.AV, eInc.BV, refDir, offset);

            if (contactPoints.Count < 2)
            {
                return(true);
            }

            offset = Vector2.Dot(eRef.BV, refDir);

            contactPoints = Clip_sat(contactPoints[0], contactPoints[1], -refDir, -offset);

            if (contactPoints.Count < 2)
            {
                return(true);
            }

            eRef.Normal = MeshUtils.CalcNormal(eRef.AV, eRef.BV);
            eInc.Normal = MeshUtils.CalcNormal(eInc.AV, eInc.BV);

            //Debug.Log(eRef.Normal);

            if (Vector2.Dot(eRef.Normal, contactPoints[1] - eRef.AV) > 0f)
            {
                contactPoints.RemoveAt(1);
            }

            if (Vector2.Dot(eRef.Normal, contactPoints[0] - eRef.AV) > 0f)
            {
                contactPoints.RemoveAt(0);
            }

            manifold.ContactPoints = contactPoints.ToArray();
            manifold.Penetration   = penetration;

            if (flipped)
            {
                manifold.Normal      = -collisionAxis.Axis;
                manifold.BodyAInc    = collisionAxis.BodyA;
                manifold.EdgeNormalA = manifold.BodyAInc ? eInc.Normal : eRef.Normal;
                manifold.EdgeNormalB = manifold.BodyAInc ? eRef.Normal : eInc.Normal;
            }
            else
            {
                manifold.Normal      = collisionAxis.Axis;
                manifold.BodyAInc    = !collisionAxis.BodyA;
                manifold.EdgeNormalA = manifold.BodyAInc ? eInc.Normal : eRef.Normal;
                manifold.EdgeNormalB = manifold.BodyAInc ? eRef.Normal : eInc.Normal;
            }


            return(true);
        }
예제 #8
0
        private void ResolveContact(CollisionContact contact, Vector2 actualCollisionNormal, Vector2 edgeNormalColl, Vector2 edgeNormal, FCollisionBodyExtractor otherObject)
        {
            float bounce       = (Bounciness + otherObject.Bounciness) * 0.5f;//Mathf.Min(Bounciness, otherObject.Bounciness);
            float frictionCoef = Mathf.Min(Roughness * 0.1f, otherObject.Body.Roughness * 0.1f);

            Vector2 fN = actualCollisionNormal * Vector2.Dot(force, actualCollisionNormal);

            Vector2 startVel         = Velocity;
            float   startAngularVel  = AngularVelocity;
            Vector2 startVelB        = otherObject.Velocity;
            float   startAngularVelB = otherObject.AngularVelocity;

            if (contact.ContactPoints != null)
            {
                for (int i = 0; i < contact.ContactPoints.Length; i++)
                {
                    Vector2 radA = contact.ContactPoints[i] - MassPoint;
                    Vector2 radB = contact.ContactPoints[i] - otherObject.Body.MassPoint;

                    Vector2 relVel = startVel + radA.Cross(startAngularVel) - startVelB - radB.Cross(startAngularVelB);

                    //Vector2 relVel = startVel - startVelB;

                    float contactVel = Vector2.Dot(relVel, actualCollisionNormal);

                    if (contactVel < 0)
                    {
                        float invMassSum = InvMass + otherObject.Body.InvMass + Mathf.Pow(radA.Cross(actualCollisionNormal), 2) * InvInertia +
                                           Mathf.Pow(radB.Cross(actualCollisionNormal), 2) * otherObject.Body.InvInertia;

                        float impulseScalar = -(1f + Bounciness) * contactVel;
                        impulseScalar /= invMassSum;
                        impulseScalar /= contact.ContactPoints.Length;

                        Vector2 impulse = actualCollisionNormal * impulseScalar;

                        ApplyImpulse(impulse, radA);

                        Vector2 velAImpulse    = startVel + InvMass * impulse;
                        float   anVelAImpulse  = startAngularVel + InvInertia * radA.Cross(impulse);
                        Vector2 velAImpulseB   = Vector2.zero;
                        float   anVelAImpulseB = 0f;

                        if (otherObject.IsRigidbody)
                        {
                            ((PhysRigidbody)otherObject.Body).ApplyImpulse(-impulse, radB);

                            velAImpulseB   = startVelB + otherObject.Body.InvMass * -impulse;
                            anVelAImpulseB = startAngularVelB + otherObject.Body.InvInertia * radB.Cross(-impulse);
                        }

                        Vector2 movementTangent = new Vector2(-edgeNormalColl.y, edgeNormalColl.x);
                        Vector2 frictionVel;
                        Vector2 friction;

                        float tanDot = Vector2.Dot(movementTangent, velAImpulse);

                        if (tanDot > 0)
                        {
                            frictionVel = (movementTangent * tanDot) / contact.ContactPoints.Length;
                            friction    = (movementTangent * frictionCoef) / contact.ContactPoints.Length;


                            if (frictionVel.sqrMagnitude > friction.sqrMagnitude)
                            {
                                ApplyImpulse(-friction, radA);
                            }
                            else
                            {
                                ApplyImpulse(-frictionVel, radA);
                            }
                        }
                        else if (tanDot < 0)
                        {
                            frictionVel = (-movementTangent * -tanDot) / contact.ContactPoints.Length;
                            friction    = (-movementTangent * frictionCoef) / contact.ContactPoints.Length;


                            if (frictionVel.sqrMagnitude > friction.sqrMagnitude)
                            {
                                ApplyImpulse(-friction, radA);
                            }
                            else
                            {
                                ApplyImpulse(-frictionVel, radA);
                            }
                        }
                    }
                }
            }

            if (Vector2.Dot(force, edgeNormalColl) < 0f)
            {
                force -= fN;
            }

            if (contact.Penetration > SLOP)
            {
                if (otherObject.IsRigidbody)
                {
                    positionCorrection.Add(actualCollisionNormal * contact.Penetration * POSITION_CORRECTION_MOD * 0.5f);
                }
                else
                {
                    positionCorrection.Add(actualCollisionNormal * contact.Penetration * POSITION_CORRECTION_MOD);
                }
            }
        }
예제 #9
0
        public void GatherCollisionInformation(PhysCompoundCollider other)
        {
            if (other is PhysRigidbody)
            {
                PhysRigidbody rigid = other as PhysRigidbody;

                for (int c1 = 0; c1 < Collider.Length; c1++)
                {
                    FAABB2D bounds = other.Collider[c1].CachedBoundsWS;

                    for (int c2 = 0; c2 < other.Collider.Length; c2++)
                    {
                        if (bounds.Intersects(other.Collider[c2].CachedBoundsWS))
                        {
                            CollisionContact manifold = null;
                            bool             isBodyA  = false;

                            if (Collider[c1].IsColliding(other.Collider[c2], out manifold, out isBodyA))
                            {
                                if (isBodyA)
                                {
                                    manifold.A = this;
                                    manifold.B = rigid;
                                }
                                else
                                {
                                    manifold.A = rigid;
                                    manifold.B = this;
                                }

                                RegisterCollision(manifold, isBodyA);
                                rigid.RegisterCollision(manifold, !isBodyA);
                            }
                        }
                    }
                }
            }
            else
            {
                for (int c1 = 0; c1 < Collider.Length; c1++)
                {
                    FAABB2D bounds = other.Collider[c1].CachedBoundsWS;

                    for (int c2 = 0; c2 < other.Collider.Length; c2++)
                    {
                        if (bounds.Intersects(other.Collider[c2].CachedBoundsWS))
                        {
                            CollisionContact manifold = null;
                            bool             isBodyA  = false;

                            if (Collider[c1].IsColliding(other.Collider[c2], out manifold, out isBodyA))
                            {
                                if (isBodyA)
                                {
                                    manifold.A = this;
                                    manifold.B = other;
                                }
                                else
                                {
                                    manifold.A = other;
                                    manifold.B = this;
                                }

                                RegisterCollision(manifold, isBodyA);
                            }
                        }
                    }
                }
            }
        }
예제 #10
0
 public virtual bool IsColliding(PhysCollider collider, out CollisionContact manifold, out bool isBodyA)
 {
     manifold = null;
     isBodyA  = true;
     return(false);
 }