2d物理引擎学习 - 两Box碰撞后反弹
效果
上图中碰撞后，两个Box开始旋转，什么情况下碰撞后会引起旋转？
作用在碰撞点的冲量方向不经过物体中点（质心）
SAT的碰撞检测就是沿用的box2d-lite的
整体代码结构和这篇类似：2d物理引擎学习 - 两圆的线性运动碰撞反馈，只是加入了角运动
关于MyRigidbody.GetKMass函数中，根据惯量计算mass部分，为啥要用勾股定理再计算出r？
假设作用力(或线冲量)方向如下图所示，那与作用力垂直的力臂长度是不是就是图中的d，d2 = r2 - dot(r, 作用力方向)2
public class MyRigidbody : MonoBehaviour { private int m_Id; [SerializeField] private Vector2 m_Size; //刚体形状现在固定为Box
//---------- 线性运动 [SerializeField] private float m_Mass; //质量 private float m_InvMass; private Vector2 m_Force; //持续作用的力 private Vector2 m_ForceImpulse; //脉冲力 [SerializeField] private Vector2 m_Velocity; //当前移动速度 [SerializeField] private Vector2 m_Position; //当前位置 //---------- //----------角运动 private float m_Inertia; //转动惯量 private float m_InvInertia; private float m_Torque; //持续作用的力矩 private float m_TorqueImpulse; //冲量矩 [SerializeField] private float m_AngleVelocity; //角速度(弧度) [SerializeField] private float m_Rotation; //旋转角度 //---------- void Start() { if (m_Mass <= 0) { Mass = float.PositiveInfinity; Inertia = float.PositiveInfinity; } else { Mass = m_Mass; Inertia = m_Mass * (m_Size.x * m_Size.x + m_Size.y * m_Size.y) / 12.0f; } } public MyRigidbody() { } public MyRigidbody(float mass, float inertia) { Mass = mass; Inertia = inertia; } public int Id { get { return m_Id; } set { m_Id = value; } } public Vector2 Size { get { return m_Size; } set { m_Size = value; } } public float Friction { get { return m_Friction; } set { m_Friction = value; } } //---------- 线性运动 public float Mass { get { return m_Mass; } set { m_Mass = value; if (value >= float.PositiveInfinity) m_InvMass = 0; else m_InvMass = 1 / value; } } public float InvMass { get { return m_InvMass; } } public Vector2 Position { get { return m_Position; } set { m_Position = value; } } public Vector2 Velocity { get { return m_Velocity; } } //线性冲量产生线速度变化 public void ApplyImpulse(Vector2 impulse) { m_Velocity += impulse * m_InvMass; // 动量定理: I = Δp = m * Δv } //---------- //----------角运动 public float Inertia { get { return m_Inertia; } set { m_Inertia = value; if (value >= float.PositiveInfinity) m_InvInertia = 0; else m_InvInertia = 1 / value; } } public float InvInertia { get { return m_InvInertia; } } public float AngleVelocity { get { return m_AngleVelocity; } } public float Rotation { get { return m_Rotation; } set { m_Rotation = value; } } /// <summary> /// 线性冲量产生角速度变化 /// </summary> /// <param name="point">力作用点</param> /// <param name="impulse">线性冲量</param> public void ApplyTorqueImpulse(Vector2 point, Vector2 impulse) { Vector2 r = point - m_Position; //角冲量: H = r × 线性冲量; r为力臂向量(或叫矢径), r与线性冲量方向垂直; float torqueImpulse = r.x * impulse.y - r.y * impulse.x; //角动量定理: H = ΔL = I * Δω //所以角速度的变化: Δω = H / I m_AngleVelocity += torqueImpulse * m_InvInertia; } //---------- public float GetKMass(Vector2 point, Vector2 normal) { float kMass = m_InvMass; Vector2 rVec = point - m_Position; float rn = Vector2.Dot(rVec, normal); float rSqr = rVec.sqrMagnitude - rn * rn; //惯量: I = m * r^2, r为与线性冲量方向垂直的力臂 // >>> 1/m = r^2 / I kMass += rSqr * m_InvInertia; return kMass; } public Vector2 GetPointVelocity(Vector2 point) { Vector2 r = point - m_Position; //角运动线速度: v = r * dir * ω, dir为线速度方向 Vector2 v = new Vector2(-r.y, r.x) * m_AngleVelocity; return m_Velocity + v; } //计算力和冲量引起的速度变化 public void PreUpdate(Vector2 gravity, float dt) { //----- 持续力 //a = F / m //v1 = v0 + a * t m_Velocity += (m_Force * m_InvMass + gravity) * dt; //角加速度 = 力矩 / 惯量 //ω1 = ω0 + 角加速度 * t m_AngleVelocity += m_Torque * m_InvInertia * dt; //----- //----- 脉冲力(冲量) //动量定理: 冲量 = Δp = m * Δv // >>> Δv = 冲量 / m m_Velocity += m_ForceImpulse * m_InvMass; //角动量定理: 角冲量 = ΔL = 惯量 * Δω // >>> Δω = 角冲量 / 惯量 m_AngleVelocity += m_TorqueImpulse * m_InvInertia; m_ForceImpulse = Vector2.zero; //冲量是瞬时效果, 作用完就置零 m_TorqueImpulse = 0; //----- } //根据速度进行运动 public void PostUpdate(float dt) { m_Position += m_Velocity * dt; m_Rotation += m_AngleVelocity * dt; } #if UNITY_EDITOR public Color m_GizmosColor = Color.white; private Vector2[] m_TempVerts = new Vector2[4]; private void UpdateCorners() { Vector2 halfSize = m_Size * 0.5f; m_TempVerts[0] = -halfSize; //lb m_TempVerts[1] = new Vector2(-halfSize.x, halfSize.y); //lt m_TempVerts[2] = halfSize; //rt m_TempVerts[3] = new Vector2(halfSize.x, -halfSize.y); //rb } public Vector2[] GetVerts() { UpdateCorners(); for (int i = 0; i < m_TempVerts.Length; ++i) m_TempVerts[i] = this.transform.TransformPoint(m_TempVerts[i]); return m_TempVerts; } private void OnDrawGizmos() { if (m_Size.sqrMagnitude <= 0) return; var oldColor = Gizmos.color; Gizmos.color = m_GizmosColor; var verts = GetVerts(); var trans = this.transform; if (Application.isPlaying) { trans.position = m_Position; trans.eulerAngles = new Vector3(0, 0, Mathf.Rad2Deg * m_Rotation); } else { m_Position = trans.position; m_Rotation = trans.eulerAngles.z * Mathf.Deg2Rad; } for (int i = 0; i < verts.Length; ++i) { var pos1 = verts[i]; var pos2 = verts[(i + 1) % verts.Length]; Gizmos.DrawLine(pos1, pos2); } Gizmos.color = oldColor; } #endif }
enum CollisionStage { None, Enter, Stay, Exit, } //CollisionPair使用两个刚体的id作为索引 struct CollisionPairKey { public int m_IdA; public int m_IdB; public CollisionPairKey(int idA, int idB) { m_IdA = idA; m_IdB = idB; } } class CollisionPair { public int m_UpdateIndex; //发生碰撞时的帧 public MyRigidbody m_RigidbodyA; public MyRigidbody m_RigidbodyB; public CollisionStage m_Stage = CollisionStage.None; public ContactInfo[] m_Contacts = new ContactInfo[2]; //box最多2个碰撞点 public int m_NumContacts; } //碰撞点信息 public class ContactInfo { public Vector2 m_Point; //碰撞点 public Vector2 m_Normal; //碰撞法向量(分离方向), 这边用A指向B, 即: B反弹方向 public float m_Penetration; //穿透深度(分离距离) }
public class MyPhysics : MonoBehaviour { [Range(0, 1)] public float m_Elasticity = 0; private List<MyRigidbody> m_RigidbodyList = new List<MyRigidbody>(); private List<MyRigidbody> m_PendingAddList = new List<MyRigidbody>(); //要添加的刚体会在下一帧添加 private List<MyRigidbody> m_PendingRemoveList = new List<MyRigidbody>(); //要删除的刚体在下一帧删除 private Dictionary<CollisionPairKey, CollisionPair> m_CollisionPairDict = new Dictionary<CollisionPairKey, CollisionPair>(); //两个发生碰撞的物体 private List<CollisionPairKey> m_TempRemoveCollisionPairList = new List<CollisionPairKey>(); private int m_IdCounter; //刚体id计数 private int m_UpdateCounter; //更新计数 private B2SatCollide m_Sat = new B2SatCollide(); void Start() { var initRigidbodys = GetComponentsInChildren<MyRigidbody>(); foreach (var rigidbody in initRigidbodys) { AddRigidbody(rigidbody); } } void FixedUpdate() { if (Time.fixedDeltaTime > 0) Step(Time.fixedDeltaTime); } public void Step(float dt) { CheckPendingList(); m_UpdateCounter++; for (int i = 0; i < m_RigidbodyList.Count; ++i) { var rigidbody = m_RigidbodyList[i]; if (0 == rigidbody.InvMass) continue; rigidbody.PreUpdate(m_Gravity, dt); } CheckCollision(); UpdateSeperation(dt); for (int i = 0; i < m_RigidbodyList.Count; ++i) { var rigidbody = m_RigidbodyList[i]; rigidbody.PostUpdate(dt); } } //检查发生碰撞的物体 private void CheckCollision() { for (int i = 0; i < m_RigidbodyList.Count; ++i) { var rigidbodyA = m_RigidbodyList[i]; for (int j = i + 1; j < m_RigidbodyList.Count; ++j) { var rigidbodyB = m_RigidbodyList[j]; if (0 == rigidbodyA.InvMass && 0 == rigidbodyB.InvMass) continue; if (m_Sat.Collide(rigidbodyA, rigidbodyB) > 0) { if (rigidbodyA.Id < rigidbodyB.Id) OnCollide(rigidbodyA, rigidbodyB); else OnCollide(rigidbodyB, rigidbodyA); } } } } //碰撞处理 private void OnCollide(MyRigidbody rigidbodyA, MyRigidbody rigidbodyB) { var key = new CollisionPairKey(rigidbodyA.Id, rigidbodyB.Id); if (!m_CollisionPairDict.TryGetValue(key, out var collisionInfo)) //之前没发生过碰撞(第1次碰撞) { collisionInfo = new CollisionPair(); collisionInfo.m_RigidbodyA = rigidbodyA; collisionInfo.m_RigidbodyB = rigidbodyB; m_CollisionPairDict.Add(key, collisionInfo); } collisionInfo.m_UpdateIndex = m_UpdateCounter; //发生了碰撞就更新帧id, 如果有一帧没更新, 就说明那一帧没发生碰撞 collisionInfo.m_NumContacts = m_Sat.NumContacts; for (int i = 0; i < collisionInfo.m_NumContacts; ++i) collisionInfo.m_Contacts[i] = m_Sat.GetContact(i); if (collisionInfo.m_Stage == CollisionStage.None) //第1次碰撞 { collisionInfo.m_Stage = CollisionStage.Enter; } } //物体发生弹性碰撞, 会相互弹开 private void UpdateSeperation(float dt) { foreach (var entry in m_CollisionPairDict) { var collisionPair = entry.Value; if (collisionPair.m_UpdateIndex != m_UpdateCounter) //上一帧没发生碰撞 { collisionPair.m_Stage = CollisionStage.Exit; } switch (collisionPair.m_Stage) { case CollisionStage.Enter: //todo: 通知Enter事件 collisionPair.m_Stage = CollisionStage.Stay; break; case CollisionStage.Exit: //todo: 通知Exit事件 collisionPair.m_Stage = CollisionStage.None; var key = new CollisionPairKey(collisionPair.m_RigidbodyA.Id, collisionPair.m_RigidbodyB.Id); m_TempRemoveCollisionPairList.Add(key); //for循环中删除会报错 break; } if (CollisionStage.Stay == collisionPair.m_Stage) { //todo: 通知Stay事件 } } if (m_TempRemoveCollisionPairList.Count > 0) { for (int i = 0; i < m_TempRemoveCollisionPairList.Count; ++i) { var key = m_TempRemoveCollisionPairList[i]; m_CollisionPairDict.Remove(key); } m_TempRemoveCollisionPairList.Clear(); } foreach (var entry in m_CollisionPairDict) { PostSeperation(dt, entry.Value); } } private void PostSeperation(float dt, CollisionPair collisionPair) { var rigidbodyA = collisionPair.m_RigidbodyA; var rigidbodyB = collisionPair.m_RigidbodyB; for (int i = 0; i < collisionPair.m_NumContacts; ++i) { var contact = collisionPair.m_Contacts[i]; var relativeV = rigidbodyB.GetPointVelocity(contact.m_Point) - rigidbodyA.GetPointVelocity(contact.m_Point); var normal = contact.m_Normal; float relativeVN = Vector2.Dot(relativeV, normal); //投影到法向量 //if (relativeVN > 0) //相对速度>0时, 表明没有碰撞趋势了 // return; float kMass = rigidbodyA.GetKMass(contact.m_Point, normal) + rigidbodyB.GetKMass(contact.m_Point, normal); float massNormal = 1 / kMass; //Δp = (1 + e) * (v2 - v1) / kMass float deltaPN = (1 + m_Elasticity) * relativeVN * massNormal; deltaPN = -deltaPN; //对Δp取反, 主要是为了让累加冲量是正值 deltaPN = Mathf.Max(deltaPN, 0); //冲量为负, 碰撞后就加速了, 这样不对 Vector2 impulseN = deltaPN * normal; //转为矢量 rigidbodyA.ApplyImpulse(-impulseN); rigidbodyA.ApplyTorqueImpulse(contact.m_Point, -impulseN); rigidbodyB.ApplyImpulse(impulseN); rigidbodyB.ApplyTorqueImpulse(contact.m_Point, impulseN); } } private void CheckPendingList() { if (m_PendingAddList.Count > 0) { for (int i = 0; i < m_PendingAddList.Count; ++i) { var rigidbody = m_PendingAddList[i]; rigidbody.Id = m_IdCounter++; rigidbody.OnAddToPhysics(); m_RigidbodyList.Add(rigidbody); } m_PendingAddList.Clear(); } if (m_PendingRemoveList.Count > 0) { for (int i = 0; i < m_PendingRemoveList.Count; ++i) { var rigidbody = m_PendingRemoveList[i]; m_RigidbodyList.Remove(rigidbody); rigidbody.OnRemoveFromPhysics(); } m_PendingRemoveList.Clear(); } } public void AddRigidbody(MyRigidbody rigidbody) { m_PendingAddList.Add(rigidbody); } public void RemoveRigidbody(MyRigidbody rigidbody) { m_PendingRemoveList.Add(rigidbody); } #if UNITY_EDITOR public Color m_GizmosColor = Color.white; private void OnDrawGizmos() { var oldColor = Gizmos.color; Gizmos.color = m_GizmosColor; Handles.color = m_GizmosColor; foreach (var entry in m_CollisionPairDict) { var collisionPair = entry.Value; for (int i = 0; i < collisionPair.m_NumContacts; ++i) { var contactInfo = collisionPair.m_Contacts[i]; DrawGizmosHelper.DrawPoint2(contactInfo.m_Point); DrawGizmosHelper.DrawArrowLine(contactInfo.m_Point, contactInfo.m_Normal, 0.5f); } } Gizmos.color = oldColor; Handles.color = oldColor; } #endif }
