diff --git a/Sources/Everglow.Core/DataStructures/UnionFind.cs b/Sources/Everglow.Core/DataStructures/UnionFind.cs new file mode 100644 index 000000000..3c46dafe1 --- /dev/null +++ b/Sources/Everglow.Core/DataStructures/UnionFind.cs @@ -0,0 +1,53 @@ +namespace Everglow.Commons.DataStructures; + +public class UnionFind +{ + private int[] parent; + private int[] rank; + + public UnionFind(int size) + { + parent = new int[size]; + rank = new int[size]; + + for (int i = 0; i < size; i++) + { + parent[i] = i; + rank[i] = 0; + } + } + + public int Find(int x) + { + if (parent[x] != x) + { + parent[x] = Find(parent[x]); + } + return parent[x]; + } + + public void Union(int x, int y) + { + int rootX = Find(x); + int rootY = Find(y); + + if (rootX == rootY) + { + return; + } + + if (rank[rootX] < rank[rootY]) + { + parent[rootX] = rootY; + } + else if (rank[rootX] > rank[rootY]) + { + parent[rootY] = rootX; + } + else + { + parent[rootY] = rootX; + rank[rootX]++; + } + } +} diff --git a/Sources/Everglow.Core/Everglow.Core.csproj b/Sources/Everglow.Core/Everglow.Core.csproj index 145d27e8c..3d6f83865 100644 --- a/Sources/Everglow.Core/Everglow.Core.csproj +++ b/Sources/Everglow.Core/Everglow.Core.csproj @@ -6,9 +6,20 @@ + + + + + + + + + + + diff --git a/Sources/Everglow.Core/Utilities/Matrix2x2.cs b/Sources/Everglow.Core/Utilities/Matrix2x2.cs new file mode 100644 index 000000000..5ed29f298 --- /dev/null +++ b/Sources/Everglow.Core/Utilities/Matrix2x2.cs @@ -0,0 +1,210 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Everglow.Commons.Utilities +{ + /// + /// 2x2 矩阵类 + /// + public class Matrix2x2 + { + public static Matrix2x2 Identity = new Matrix2x2( + new double[2, 2] + { + { 1, 0}, { 0, 1 }, + }); + public static Matrix2x2 Zero = new Matrix2x2( + new double[2, 2] + { + { 0, 0 }, { 0, 0 }, + }); + + private double[,] _matrix; + + public Matrix2x2() + { + _matrix = new double[2, 2]; + } + + public Matrix2x2(double[,] initialMatrix) + { + if (initialMatrix.GetLength(0) != 2 || initialMatrix.GetLength(1) != 2) + throw new ArgumentException("Initial matrix must be 2x2"); + + _matrix = initialMatrix; + } + + public double this[int row, int col] + { + get + { + return _matrix[row, col]; + } + set + { + _matrix[row, col] = value; + } + } + + public double Trace() + { + return _matrix[0, 0] + _matrix[1, 1]; + } + + public Matrix2x2 Inverse() + { + double det = Determinant(); + if (det == 0) + throw new InvalidOperationException("Matrix is not invertible."); + + var result = new Matrix2x2 + { + [0, 0] = _matrix[1, 1] / det, + [0, 1] = -_matrix[0, 1] / det, + [1, 0] = -_matrix[1, 0] / det, + [1, 1] = _matrix[0, 0] / det + }; + + return result; + } + + public Matrix2x2 Multiply(Matrix2x2 other) + { + var result = new Matrix2x2 + { + [0, 0] = this[0, 0] * other[0, 0] + this[0, 1] * other[1, 0], + [0, 1] = this[0, 0] * other[0, 1] + this[0, 1] * other[1, 1], + [1, 0] = this[1, 0] * other[0, 0] + this[1, 1] * other[1, 0], + [1, 1] = this[1, 0] * other[0, 1] + this[1, 1] * other[1, 1] + }; + + return result; + } + + public Vector2 Multiply(Vector2 other) + { + return new Vector2((float)(this[0, 0] * other.X + this[0, 1] * other.Y), + (float)(this[1, 0] * other.X + this[1, 1] * other.Y)); + } + + public double Determinant() + { + return _matrix[0, 0] * _matrix[1, 1] - _matrix[0, 1] * _matrix[1, 0]; + } + + public Matrix2x2 Adjoint() + { + return new Matrix2x2 + { + [0, 0] = _matrix[1, 1], + [0, 1] = -_matrix[0, 1], + [1, 0] = -_matrix[1, 0], + [1, 1] = _matrix[0, 0], + }; + } + + public static Matrix2x2 operator *(Matrix2x2 a, double b) + { + var result = new Matrix2x2 + { + [0, 0] = a[0, 0] * b, + [0, 1] = a[0, 1] * b, + [1, 0] = a[1, 0] * b, + [1, 1] = a[1, 1] * b + }; + + return result; + } + + public static Matrix2x2 operator *(double b, Matrix2x2 a) + { + var result = new Matrix2x2 + { + [0, 0] = a[0, 0] * b, + [0, 1] = a[0, 1] * b, + [1, 0] = a[1, 0] * b, + [1, 1] = a[1, 1] * b + }; + + return result; + } + + public static Matrix2x2 operator *(Matrix2x2 a, Matrix2x2 b) + { + var result = new Matrix2x2 + { + [0, 0] = a[0, 0] * b[0, 0] + a[0, 1] * b[1, 0], + [0, 1] = a[0, 0] * b[0, 1] + a[0, 1] * b[1, 1], + [1, 0] = a[1, 0] * b[0, 0] + a[1, 1] * b[1, 0], + [1, 1] = a[1, 0] * b[0, 1] + a[1, 1] * b[1, 1] + }; + + return result; + } + + public static Matrix2x2 operator +(Matrix2x2 a, Matrix2x2 b) + { + var result = new Matrix2x2 + { + [0, 0] = a[0, 0] + b[0, 0], + [0, 1] = a[0, 1] + b[0, 1], + [1, 0] = a[1, 0] + b[1, 0], + [1, 1] = a[1, 1] + b[1, 1] + }; + + return result; + } + + public static Matrix2x2 operator -(Matrix2x2 a, Matrix2x2 b) + { + var result = new Matrix2x2 + { + [0, 0] = a[0, 0] - b[0, 0], + [0, 1] = a[0, 1] - b[0, 1], + [1, 0] = a[1, 0] - b[1, 0], + [1, 1] = a[1, 1] - b[1, 1] + }; + + return result; + } + + public static Matrix2x2 operator /(Matrix2x2 a, Matrix2x2 b) + { + var result = new Matrix2x2 + { + [0, 0] = a[0, 0] / b[0, 0], + [0, 1] = a[0, 1] / b[0, 1], + [1, 0] = a[1, 0] / b[1, 0], + [1, 1] = a[1, 1] / b[1, 1] + }; + + return result; + } + + public Matrix2x2 Transpose() + { + return new Matrix2x2 + { + [0, 0] = _matrix[0, 0], + [0, 1] = _matrix[1, 0], + [1, 0] = _matrix[0, 1], + [1, 1] = _matrix[1, 1], + }; + } + + + public static Matrix2x2 CreateRotationMatrix(float r) + { + return new Matrix2x2 + { + [0, 0] = Math.Cos(r), + [0, 1] = -Math.Sin(r), + [1, 0] = Math.Sin(r), + [1, 1] = Math.Cos(r), + }; + } + } +} diff --git a/Sources/Everglow.Core/Utilities/Matrix3x3.cs b/Sources/Everglow.Core/Utilities/Matrix3x3.cs new file mode 100644 index 000000000..6180665ee --- /dev/null +++ b/Sources/Everglow.Core/Utilities/Matrix3x3.cs @@ -0,0 +1,217 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Everglow.Commons.Utilities +{ + public class Matrix3x3 + { + public static Matrix3x3 Identity = new Matrix3x3(new Vector3(1, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 1)); + private double[,] _matrix; + + public Matrix3x3() + { + _matrix = new double[3, 3]; + } + public Matrix3x3(double[,] initialMatrix) + { + if (initialMatrix.GetLength(0) != 3 || initialMatrix.GetLength(1) != 3) + throw new ArgumentException("Initial matrix must be 3x3"); + + _matrix = initialMatrix; + } + + public double this[int row, int col] + { + get + { + return _matrix[row, col]; + } + set + { + _matrix[row, col] = value; + } + } + + public Matrix3x3(Vector3 col0, Vector3 col1, Vector3 col2) + { + _matrix = new double[3, 3]; + _matrix[0, 0] = col0.X; + _matrix[1, 0] = col0.Y; + _matrix[2, 0] = col0.Z; + + _matrix[0, 1] = col1.X; + _matrix[1, 1] = col1.Y; + _matrix[2, 1] = col1.Z; + + _matrix[0, 2] = col2.X; + _matrix[1, 2] = col2.Y; + _matrix[2, 2] = col2.Z; + } + + public static Matrix3x3 operator *(Matrix3x3 a, double x) + { + double[,] data = new double[3, 3]; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + data[i, j] = a._matrix[i, j] * x; + } + } + + return new Matrix3x3(data); + } + + public static Matrix3x3 operator -(Matrix3x3 a, Matrix3x3 b) + { + double[,] data = new double[3, 3]; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + data[i, j] = a._matrix[i, j] - b._matrix[i, j]; + } + } + + return new Matrix3x3(data); + } + + public static Matrix3x3 operator +(Matrix3x3 a, Matrix3x3 b) + { + double[,] data = new double[3, 3]; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + data[i, j] = a._matrix[i, j] + b._matrix[i, j]; + } + } + + return new Matrix3x3(data); + } + + public static Matrix3x3 operator /(Matrix3x3 a, Matrix3x3 b) + { + double[,] data = new double[3, 3]; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + data[i, j] = a._matrix[i, j] / b._matrix[i, j]; + } + } + + return new Matrix3x3(data); + } + + + + public Matrix3x3 Transpose() + { + double[,] m = new double[3, 3]; + m[0, 0] = _matrix[0, 0]; + m[0, 1] = _matrix[1, 0]; + m[0, 2] = _matrix[2, 0]; + + m[1, 0] = _matrix[0, 1]; + m[1, 1] = _matrix[1, 1]; + m[1, 2] = _matrix[2, 1]; + + m[2, 0] = _matrix[0, 2]; + m[2, 1] = _matrix[1, 2]; + m[2, 2] = _matrix[2, 2]; + return new Matrix3x3(m); + } + + public Matrix3x3 Multiply(Matrix3x3 other) + { + double[,] result = new double[3, 3]; + + result[0, 0] = _matrix[0, 0] * other._matrix[0, 0] + _matrix[0, 1] * other._matrix[1, 0] + + _matrix[0, 2] * other._matrix[2, 0]; + result[0, 1] = _matrix[0, 0] * other._matrix[0, 1] + _matrix[0, 1] * other._matrix[1, 1] + + _matrix[0, 2] * other._matrix[2, 1]; + result[0, 2] = _matrix[0, 0] * other._matrix[0, 2] + _matrix[0, 1] * other._matrix[1, 2] + + _matrix[0, 2] * other._matrix[2, 2]; + + result[1, 0] = _matrix[1, 0] * other._matrix[0, 0] + _matrix[1, 1] * other._matrix[1, 0] + + _matrix[1, 2] * other._matrix[2, 0]; + result[1, 1] = _matrix[1, 0] * other._matrix[0, 1] + _matrix[1, 1] * other._matrix[1, 1] + + _matrix[1, 2] * other._matrix[2, 1]; + result[1, 2] = _matrix[1, 0] * other._matrix[0, 2] + _matrix[1, 1] * other._matrix[1, 2] + + _matrix[1, 2] * other._matrix[2, 2]; + + result[2, 0] = _matrix[2, 0] * other._matrix[0, 0] + _matrix[2, 1] * other._matrix[1, 0] + + _matrix[2, 2] * other._matrix[2, 0]; + result[2, 1] = _matrix[2, 0] * other._matrix[0, 1] + _matrix[2, 1] * other._matrix[1, 1] + + _matrix[2, 2] * other._matrix[2, 1]; + result[2, 2] = _matrix[2, 0] * other._matrix[0, 2] + _matrix[2, 1] * other._matrix[1, 2] + + _matrix[2, 2] * other._matrix[2, 2]; + + return new Matrix3x3(result); + } + + public Vector3 Multiply(Vector3 v) + { + return new Vector3((float)(_matrix[0, 0] * v.X + _matrix[0, 1] * v.Y + _matrix[0, 2] * v.Z), + (float)(_matrix[1, 0] * v.X + _matrix[1, 1] * v.Y + _matrix[1, 2] * v.Z), + (float)(_matrix[2, 0] * v.X + _matrix[2, 1] * v.Y + _matrix[2, 2] * v.Z)); + } + + public double Determinant() + { + return _matrix[0, 0] * (_matrix[1, 1] * _matrix[2, 2] - _matrix[1, 2] * _matrix[2, 1]) - + _matrix[0, 1] * (_matrix[1, 0] * _matrix[2, 2] - _matrix[1, 2] * _matrix[2, 0]) + + _matrix[0, 2] * (_matrix[1, 0] * _matrix[2, 1] - _matrix[1, 1] * _matrix[2, 0]); + } + + public Matrix3x3 Inverse() + { + double det = Determinant(); + if (Math.Abs(det) < 1e-7) + throw new InvalidOperationException("Non-invertible matrix"); + + double[,] result = new double[3, 3]; + + result[0, 0] = (_matrix[1, 1] * _matrix[2, 2] - _matrix[1, 2] * _matrix[2, 1]) / det; + result[0, 1] = (_matrix[0, 2] * _matrix[2, 1] - _matrix[0, 1] * _matrix[2, 2]) / det; + result[0, 2] = (_matrix[0, 1] * _matrix[1, 2] - _matrix[0, 2] * _matrix[1, 1]) / det; + result[1, 0] = (_matrix[1, 2] * _matrix[2, 0] - _matrix[1, 0] * _matrix[2, 2]) / det; + result[1, 1] = (_matrix[0, 0] * _matrix[2, 2] - _matrix[0, 2] * _matrix[2, 0]) / det; + result[1, 2] = (_matrix[0, 2] * _matrix[1, 0] - _matrix[0, 0] * _matrix[1, 2]) / det; + result[2, 0] = (_matrix[1, 0] * _matrix[2, 1] - _matrix[1, 1] * _matrix[2, 0]) / det; + result[2, 1] = (_matrix[0, 1] * _matrix[2, 0] - _matrix[0, 0] * _matrix[2, 1]) / det; + result[2, 2] = (_matrix[0, 0] * _matrix[1, 1] - _matrix[0, 1] * _matrix[1, 0]) / det; + + return new Matrix3x3(result); + } + + public double Trace() + { + return _matrix[0, 0] + _matrix[1, 1] + _matrix[2, 2]; + } + + public Vector3 Column(int i) + { + return new Vector3((float)_matrix[0, i], (float)_matrix[1, i], (float)_matrix[2, i]); + } + + public bool HasNaN() + { + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + if (double.IsNaN(_matrix[i, j])) + { + return true; + } + } + } + return false; + } + } +} diff --git a/Sources/Everglow.Core/Utilities/README.md b/Sources/Everglow.Core/Utilities/README.md new file mode 100644 index 000000000..8858e84fc --- /dev/null +++ b/Sources/Everglow.Core/Utilities/README.md @@ -0,0 +1,19 @@ +# Utils简介 +一些与Tr无关的常用函数放在这里，目前我准备大致分为以下几类 +目前firefly分支未完全合并，同时暂未想到很常用的方法，所以内容先搁置了（） +如果以后某个Utils多了会考虑继续细分，比如几何处理，物理模拟等 + +## MathUtils +一些常用的数学计算，处理等 + +## XNAUtils +对于XNA一些方法的进一步包装，扩展等 + +## CSUtils +对于System的一些方法的包装扩展，比如一堆麻烦的反射什么的（） + +## Matrix2x2 +包含2x2矩阵的实现，支持常用矩阵操作，包括转置、求逆、迹、旋转等等 + +## Matrix3x3 +包含2x2矩阵的实现，支持常用矩阵操作，包括转置、求逆、迹、旋转等等 diff --git a/Sources/Everglow.Core/Utilities/Utils.md b/Sources/Everglow.Core/Utilities/Utils.md deleted file mode 100644 index d5b68ab97..000000000 --- a/Sources/Everglow.Core/Utilities/Utils.md +++ /dev/null @@ -1,13 +0,0 @@ -# Utils��� -һЩ��Tr�޹صij��ú����������Ŀǰ��׼�����·�Ϊ���¼��� -Ŀǰfirefly��֧δ��ȫ�ϲ���ͬʱ��δ�뵽�ܳ��õķ��������������ȸ����ˣ��� -����Ժ�ij��Utils���˻ῼ�Ǽ���ϸ�֣����缸�δ���������ģ��� - -## MathUtils -һЩ���õ���ѧ���㣬������ - -## XNAUtils -����XNAһЩ�����Ľ�һ����װ����չ�� - -## CSUtils -����System��һЩ�����İ�װ��չ������һ���鷳�ķ���ʲô�ģ��� diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/BVHDetect.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/BVHDetect.cs new file mode 100644 index 000000000..f846361e8 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/BVHDetect.cs @@ -0,0 +1,173 @@ +using Everglow.Commons.Physics.PBEngine.Collision.BroadPhase.Structure; +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Terraria.GameContent; +using Terraria; +using Everglow.Commons.Physics.PBEngine.Core; + +namespace Everglow.Commons.Physics.PBEngine.Collision.BroadPhase +{ + /// + /// 使用BVH辅助加速碰撞检测 + /// + public class BVHDetect : BroadPhase + { + private List _colliders; + private Dictionary> _groups; + private Dictionary _bvhForGroup; + public BVHDetect(CollisionGraph graph) : base(graph) + { + _colliders = new List(); + _groups = new Dictionary>(); + _bvhForGroup = new Dictionary(); + } + + private void GroupInnerDetection(string name, + List> collisionResults) + { + var pairs = _bvhForGroup[name].QueryPairs(); + foreach (var pair in pairs) + { + collisionResults.Add(new KeyValuePair(_colliders[pair.Key], + _colliders[pair.Value])); + } + } + + private void GroupOuterDetection(string groupA, string groupB, + List> collisionResults) + { + var groupAEntries = _groups[groupA]; + var groupBBVH = _bvhForGroup[groupB]; + for (int i = 0; i < groupAEntries.Count; i++) + { + foreach (var id in groupBBVH.QueryRange(groupAEntries[i].BoundingBox)) + { + if (groupAEntries[i].IsDynamic || _colliders[id].ParentObject.RigidBody.MovementType == MovementType.Player || + _colliders[id].ParentObject.RigidBody.MovementType == MovementType.Dynamic) + { + collisionResults.Add(new KeyValuePair(_colliders[groupAEntries[i].ColliderId], + _colliders[id])); + } + } + } + } + + public override List> GetCollisionPairs(float deltaTime) + { + List> finalPairs = new List>(); + foreach (var group in _groups) + { + var thisgroup = _groups[group.Key]; + if (_collisionGraph.Graph.ContainsKey(group.Key)) + { + foreach (var dual in _collisionGraph.Graph[group.Key]) + { + if (dual == group.Key) + { + GroupInnerDetection(group.Key, finalPairs); + continue; + } + if (!_groups.ContainsKey(dual)) + continue; + GroupOuterDetection(group.Key, dual, finalPairs); + } + } + } + return finalPairs; + } + + public override void Prepare(List objects, float deltaTime) + { + _colliders.Clear(); + _colliders.EnsureCapacity(objects.Count); + _groups.Clear(); + _bvhForGroup.Clear(); + for (int i = 0; i < objects.Count; i++) + { + var obj = objects[i]; + _colliders.Add(obj.Collider); + var entry = new ColliderEntry() + { + ColliderId = i, + BoundingBox = obj.Collider.GetAABB(deltaTime), + IsDynamic = obj.RigidBody.MovementType == MovementType.Dynamic || obj.RigidBody.MovementType == MovementType.Player + }; + + if (_groups.ContainsKey(obj.Tag)) + { + _groups[obj.Tag].Add(entry); + } + else + { + _groups.Add(obj.Tag, new List() + { + entry + }); + } + } + foreach (var group in _groups) + { + _bvhForGroup.Add(group.Key, new BVH(group.Value)); + } + } + + public override List GetSingleCollision(AABB aabb, Vector2 velocity, float deltaTime, List targetTags) + { + List colliders = new List(); + foreach (var tag in targetTags) + { + if (!_groups.ContainsKey(tag)) + { + continue; + } + var bvhThisGroup = _bvhForGroup[tag]; + + foreach (var id in bvhThisGroup.QueryRange(aabb)) + { + colliders.Add(_colliders[id]); + } + } + return colliders; + } + + public override void DrawDebugInfo(SpriteBatch sb) + { + if (!_groups.ContainsKey("Default")) + return; + //var data = _bvhForGroup["Default"].GetProfilingData(); + //sb.Begin(); + //foreach (var entry in data) + //{ + // int x = (int)(entry.GridBox.MinPoint.X - Main.screenPosition.X); + // int y = (int)(-entry.GridBox.MaxPoint.Y - Main.screenPosition.Y); + // int w = (int)(entry.GridBox.MaxPoint.X - entry.GridBox.MinPoint.X); + // int h = (int)(entry.GridBox.MaxPoint.Y - entry.GridBox.MinPoint.Y); + // sb.Draw(TextureAssets.MagicPixel.Value, new Rectangle(x, y, w, h), Color.White * MathHelper.Lerp(0.1f, 0.1f, entry.Layer / 32f)); + //} + + //sb.End(); + } + + public override bool TestSingleCollision(AABB aabb, Vector2 velocity, float deltaTime, List targetTags) + { + foreach (var tag in targetTags) + { + if (!_groups.ContainsKey(tag)) + { + continue; + } + var bvhThisGroup = _bvhForGroup[tag]; + + if (bvhThisGroup.QueryRange(aabb).Count > 0) + { + return true; + } + } + return false; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/BroadPhase.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/BroadPhase.cs new file mode 100644 index 000000000..b3ffe925d --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/BroadPhase.cs @@ -0,0 +1,33 @@ +using Everglow.Commons.Physics.PBEngine.Collision.BroadPhase.Structure; +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using Everglow.Commons.Physics.PBEngine.Core; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Everglow.Commons.Physics.PBEngine.Collision.BroadPhase +{ + /// + /// 粗粒度碰撞检测的基类 + /// + public abstract class BroadPhase + { + protected CollisionGraph _collisionGraph; + public BroadPhase(CollisionGraph graph) + { + _collisionGraph = graph; + } + public abstract void Prepare(List objects, float deltaTime); + + public abstract List> GetCollisionPairs(float deltaTime); + + public abstract List GetSingleCollision(AABB aabb, Vector2 velocity, float deltaTime, List targetTags); + + public abstract bool TestSingleCollision(AABB aabb, Vector2 velocity, float deltaTime, List targetTags); + + + public abstract void DrawDebugInfo(SpriteBatch sb); + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/BruteForceDetect.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/BruteForceDetect.cs new file mode 100644 index 000000000..820fb24e4 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/BruteForceDetect.cs @@ -0,0 +1,162 @@ +using Everglow.Commons.Physics.PBEngine.Collision.BroadPhase.Structure; +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using Everglow.Commons.Physics.PBEngine.Core; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Everglow.Commons.Physics.PBEngine.Collision.BroadPhase +{ + /// + /// 暴力遍历AABB的粗粒度碰撞检测算法 + /// + public class BruteForceDetect : BroadPhase + { + private List _colliders; + private Dictionary> _groups; + public BruteForceDetect(CollisionGraph graph) : base(graph) + { + _colliders = new List(); + _groups = new Dictionary>(); + } + + private void GroupInnerDetection(List entries, + List> collisionResults) + { + for (int i = 0; i < entries.Count; i++) + { + for (int j = i + 1; j < entries.Count; j++) + { + if ((entries[i].IsDynamic || entries[j].IsDynamic) + && entries[i].BoundingBox.Intersects(entries[j].BoundingBox)) + { + collisionResults.Add(new KeyValuePair(_colliders[entries[i].ColliderId], + _colliders[entries[j].ColliderId])); + } + } + } + } + + private void GroupOuterDetection(List groupA, + List groupB, + List> collisionResults) + { + for (int i = 0; i < groupA.Count; i++) + { + for (int j = 0; j < groupB.Count; j++) + { + if ((groupA[i].IsDynamic || groupB[j].IsDynamic) + && groupA[i].BoundingBox.Intersects(groupB[j].BoundingBox)) + { + collisionResults.Add(new KeyValuePair(_colliders[groupA[i].ColliderId], + _colliders[groupB[j].ColliderId])); + } + } + } + } + + public override List> GetCollisionPairs(float deltaTime) + { + List> finalPairs = new List>(); + foreach (var group in _groups) + { + var thisgroup = _groups[group.Key]; + + if (_collisionGraph.Graph.ContainsKey(group.Key)) + { + foreach (var dual in _collisionGraph.Graph[group.Key]) + { + if (dual == group.Key) + { + GroupInnerDetection(thisgroup, finalPairs); + continue; + } + if (!_groups.ContainsKey(dual)) + continue; + GroupOuterDetection(thisgroup, _groups[dual], finalPairs); + } + } + } + return finalPairs; + } + + public override void Prepare(List objects, float deltaTime) + { + _colliders.Clear(); + _colliders.EnsureCapacity(objects.Count); + _groups.Clear(); + for (int i = 0; i < objects.Count; i++) + { + var obj = objects[i]; + _colliders.Add(obj.Collider); + var entry = new ColliderEntry() + { + ColliderId = i, + BoundingBox = obj.Collider.GetAABB(deltaTime), + IsDynamic = obj.RigidBody.MovementType == MovementType.Dynamic || obj.RigidBody.MovementType == MovementType.Player + }; + + if (_groups.ContainsKey(obj.Tag)) + { + _groups[obj.Tag].Add(entry); + } + else + { + _groups.Add(obj.Tag, new List() + { + entry + }); + } + } + } + public override List GetSingleCollision(AABB aabb, Vector2 velocity, float deltaTime, List targetTags) + { + List colliders = new List(); + foreach (var tag in targetTags) + { + if (!_groups.ContainsKey(tag)) + { + continue; + } + var thisgroup = _groups[tag]; + + foreach (var entry in thisgroup) + { + if (entry.BoundingBox.Intersects(aabb)) + { + colliders.Add(_colliders[entry.ColliderId]); + } + } + } + return colliders; + } + + public override void DrawDebugInfo(SpriteBatch sb) + { + + } + + public override bool TestSingleCollision(AABB aabb, Vector2 velocity, float deltaTime, List targetTags) + { + foreach (var tag in targetTags) + { + if (!_groups.ContainsKey(tag)) + { + continue; + } + var thisgroup = _groups[tag]; + + foreach (var entry in thisgroup) + { + if (entry.BoundingBox.Intersects(aabb)) + { + return true; + } + } + } + return false; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/HashGridMethod.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/HashGridMethod.cs new file mode 100644 index 000000000..449f866e2 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/HashGridMethod.cs @@ -0,0 +1,159 @@ +using Everglow.Commons.Physics.PBEngine.Collision.BroadPhase.Structure; +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using Everglow.Commons.Physics.PBEngine.Core; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Terraria; +using Terraria.GameContent; + +namespace Everglow.Commons.Physics.PBEngine.Collision.BroadPhase +{ + internal class HashGridMethod : BroadPhase + { + private List _colliders; + private Dictionary _groups; + private int _curHashGridSize; + private int _prevHashGridSize; + private long _prevPairTime; + private long _prev2PairTime; + + public HashGridMethod(CollisionGraph graph) : base(graph) + { + _colliders = new List(); + _groups = new Dictionary(); + _prevHashGridSize = 256; + _curHashGridSize = 256; + + _prev2PairTime = long.MaxValue; + _prevPairTime = long.MinValue; + } + + public override void DrawDebugInfo(SpriteBatch sb) + { + if (!_groups.ContainsKey("Default")) + return; + //var data = _groups["Default"].GetProfilingData(); + //sb.Begin(); + //foreach (var entry in data) + //{ + // int x = (int)(entry.GridBox.MinPoint.X - Main.screenPosition.X); + // int y = (int)(-entry.GridBox.MaxPoint.Y - Main.screenPosition.Y); + // int w = (int)(entry.GridBox.MaxPoint.X - entry.GridBox.MinPoint.X); + // int h = (int)(entry.GridBox.MaxPoint.Y - entry.GridBox.MinPoint.Y); + // sb.Draw(TextureAssets.MagicPixel.Value, new Rectangle(x, y, w, h), Color.White * MathHelper.Lerp(0.1f, 0.5f, entry.NumObjects / 64f)); + //} + + //sb.End(); + } + + public override List> GetCollisionPairs(float deltaTime) + { + Stopwatch sw = Stopwatch.StartNew(); + List> finalPairs = new List>(); + foreach (var group in _groups) + { + var thisgroup = _groups[group.Key]; + + if (_collisionGraph.Graph.ContainsKey(group.Key)) + { + foreach (var dual in _collisionGraph.Graph[group.Key]) + { + if (dual == group.Key) + { + foreach (var pair in thisgroup.QueryPairs()) + { + finalPairs.Add(new KeyValuePair(_colliders[pair.Key], + _colliders[pair.Value])); + } + continue; + } + if (!_groups.ContainsKey(dual)) + continue; + foreach (var pair in thisgroup.QueryPairsWith(_groups[dual])) + { + finalPairs.Add(new KeyValuePair(_colliders[pair.Key], + _colliders[pair.Value])); + } + } + } + } + sw.Stop(); + _prev2PairTime = _prevPairTime; + _prevPairTime = sw.ElapsedTicks; + return finalPairs; + } + + public override List GetSingleCollision(AABB aabb, Vector2 velocity, float deltaTime, List targetTags) + { + List colliders = new List(); + foreach (var tag in targetTags) + { + if (!_groups.ContainsKey(tag)) + { + continue; + } + + foreach (var id in _groups[tag].QueryRange(aabb)) + { + colliders.Add(_colliders[id]); + } + } + return colliders; + } + + + public override void Prepare(List objects, float deltaTime) + { + _colliders.Clear(); + _colliders.EnsureCapacity(objects.Count); + _groups.Clear(); + + for (int i = 0; i < objects.Count; i++) + { + var obj = objects[i]; + _colliders.Add(obj.Collider); + var entry = new ColliderEntry() + { + ColliderId = i, + BoundingBox = obj.Collider.GetAABB(deltaTime), + IsDynamic = obj.RigidBody.MovementType == MovementType.Dynamic || obj.RigidBody.MovementType == MovementType.Player + }; + + if (!_groups.ContainsKey(obj.Tag)) + { + _groups.Add(obj.Tag, new MultiLevelHashGrid(4, new int[] + { + 1024, + 256, + 64, + 16 + })); + + } + _groups[obj.Tag].Add(entry); + } + + // _prevHashGridSize = _curHashGridSize; + } + + public override bool TestSingleCollision(AABB aabb, Vector2 velocity, float deltaTime, List targetTags) + { + foreach (var tag in targetTags) + { + if (!_groups.ContainsKey(tag)) + { + continue; + } + + if (_groups[tag].QueryRange(aabb).Count > 0) + { + return true; + } + } + return false; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/Structure/BVH.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/Structure/BVH.cs new file mode 100644 index 000000000..e982229f0 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/Structure/BVH.cs @@ -0,0 +1,399 @@ +using Everglow.Commons.Physics.PBEngine.Collision; +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using MonoMod.Cil; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Terraria; + +namespace Everglow.Commons.Physics.PBEngine.Collision.BroadPhase.Structure +{ + public struct ProfilingDataBVH + { + public AABB GridBox; + public int Layer; + } + public class BVHNode + { + public AABB Box + { + get; set; + } + public BVHNode Left + { + get; set; + } + public BVHNode Right + { + get; set; + } + public int SplitIndex + { + get; set; + } + public List Colliders + { + get; set; + } + public bool Checked + { + get; set; + } + + public BVHNode() + { + Colliders = new List(); + Left = null; + Right = null; + SplitIndex = -1; + Checked = false; + } + } + + public class BVH + { + public BVHNode Root + { + get; private set; + } + + private List _colliders; + private List> _pairQueryCache; + + private const int MAX_COLLIDERS_PER_NODE = 1; + + public BVH(List colliders) + { + _colliders = colliders; + _pairQueryCache = new List>(); + Root = _build(0, _colliders.Count - 1); + + //foreach (ColliderEntry collider in _colliders) + //{ + // Insert(collider); + //} + } + + + public void Insert(in ColliderEntry entry) + { + if (Root == null) + { + Root = new BVHNode() + { + Box = entry.BoundingBox, + Colliders = new List { entry }, + }; + return; + } + _insert(Root, entry); + } + + private void _insert(BVHNode node, in ColliderEntry entry) + { + if (node.Colliders.Count == 0) + { + var newLeftBox = node.Left.Box.Merge(entry.BoundingBox); + var newRightBox = node.Right.Box.Merge(entry.BoundingBox); + if (newLeftBox.GetArea() - node.Left.Box.GetArea() + < newRightBox.GetArea() - node.Right.Box.GetArea()) + { + _insert(node.Left, entry); + } + else + { + _insert(node.Right, entry); + } + node.Box = node.Left.Box.Merge(node.Right.Box); + } + else + { + node.Left = new BVHNode() + { + Box = node.Colliders[0].BoundingBox, + Colliders = new List() { node.Colliders[0] } + }; + + node.Right = new BVHNode() + { + Box = entry.BoundingBox, + Colliders = new List() { entry } + }; + + node.Colliders.Clear(); + node.Box = node.Left.Box.Merge(node.Right.Box); + } + } + + private BVHNode _build(int l, int r) + { + BVHNode node = new BVHNode(); + if (r - l + 1 <= MAX_COLLIDERS_PER_NODE) + { + node.Colliders = _colliders.GetRange(l, r - l + 1); + AABB aabb = new AABB(); + for (int i = l; i <= r; i++) + { + aabb.MergeWith(_colliders[i].BoundingBox); + } + node.Box = aabb; + return node; + } + + AABB fullBox = new AABB(); + for (int i = l; i <= r; i++) + { + fullBox.MergeWith(_colliders[i].BoundingBox); + } + + var sortByX = Comparer.Create((a, b) => + { + return a.BoundingBox.Center.X.CompareTo(b.BoundingBox.Center.X); + }); + var sortByY = Comparer.Create((a, b) => + { + return a.BoundingBox.Center.Y.CompareTo(b.BoundingBox.Center.Y); + }); + Comparer compFunc = (fullBox.MaxPoint.Y - fullBox.MinPoint.Y) > (fullBox.MaxPoint.X - fullBox.MinPoint.X) + ? sortByY : sortByX; + _colliders.Sort(l, r - l + 1, compFunc); + + int mid = l + (r - l) / 2; + + node.Left = _build(l, mid); + node.Right = _build(mid + 1, r); + node.Box = node.Left.Box.Merge(node.Right.Box); + node.SplitIndex = mid; + return node; + } + + private void _clearFlags(BVHNode node) + { + if (node == null) + return; + node.Checked = false; + _clearFlags(node.Left); + _clearFlags(node.Right); + } + + private void _innerPairQuery(BVHNode node) + { + if (node == null) + return; + if (node.Colliders.Count != 0) + { + int sz = node.Colliders.Count; + for (int i = 0; i < sz; i++) + { + for (int j = i + 1; j < sz; j++) + { + if ((node.Colliders[i].IsDynamic || node.Colliders[j].IsDynamic) + && node.Colliders[i].BoundingBox.Intersects(node.Colliders[j].BoundingBox)) + { + _pairQueryCache.Add(new KeyValuePair(node.Colliders[i].ColliderId, + node.Colliders[j].ColliderId)); + } + } + } + return; + } + _innerPairQuery(node.Left); + _innerPairQuery(node.Right); + } + private void _outerPairQuery(BVHNode node1, BVHNode node2) + { + int sz1 = node1.Colliders.Count; + int sz2 = node2.Colliders.Count; + for (int i = 0; i < sz1; i++) + { + for (int j = 0; j < sz2; j++) + { + if ((node1.Colliders[i].IsDynamic || node2.Colliders[j].IsDynamic) + && node1.Colliders[i].BoundingBox.Intersects(node2.Colliders[j].BoundingBox)) + { + _pairQueryCache.Add(new KeyValuePair(node1.Colliders[i].ColliderId, + node2.Colliders[j].ColliderId)); + } + } + } + } + + private void _queryPairHelper(BVHNode node1, BVHNode node2) + { + + if (node1.Colliders.Count > 0 && node2.Colliders.Count > 0) + { + _outerPairQuery(node1, node2); + return; + } + if (node1.Colliders.Count > 0 && node2.Colliders.Count == 0) + { + if (node1.Box.Intersects(node2.Left.Box)) + { + _queryPairHelper(node1, node2.Left); + } + if (node1.Box.Intersects(node2.Right.Box)) + { + _queryPairHelper(node1, node2.Right); + } + if (!node2.Checked) + { + node2.Checked = true; + _queryPairHelper(node2.Left, node2.Right); + } + return; + } + if (node2.Colliders.Count > 0 && node1.Colliders.Count == 0) + { + if (node2.Box.Intersects(node1.Left.Box)) + { + _queryPairHelper(node2, node1.Left); + } + if (node2.Box.Intersects(node1.Right.Box)) + { + _queryPairHelper(node2, node1.Right); + } + if (!node1.Checked) + { + node1.Checked = true; + _queryPairHelper(node1.Left, node1.Right); + } + return; + } + + if (node1.Box.Intersects(node2.Box)) + { + _queryPairHelper(node1.Left, node2.Left); + _queryPairHelper(node1.Right, node2.Left); + _queryPairHelper(node1.Left, node2.Right); + _queryPairHelper(node1.Right, node2.Right); + } + if (!node1.Checked) + { + node1.Checked = true; + _queryPairHelper(node1.Left, node1.Right); + } + if (!node2.Checked) + { + node2.Checked = true; + _queryPairHelper(node2.Left, node2.Right); + } + + } + + private void _queryPair(BVHNode node, int l, int r) + { + // If is a leaf node + if (node.Colliders.Count > 0) + { + for (int i = 0; i < node.Colliders.Count; i++) + { + for (int j = i + 1; j < node.Colliders.Count; j++) + { + if ((node.Colliders[i].IsDynamic || node.Colliders[j].IsDynamic) + && node.Colliders[i].BoundingBox.Intersects(node.Colliders[j].BoundingBox)) + { + _pairQueryCache.Add(new KeyValuePair(node.Colliders[i].ColliderId, + node.Colliders[j].ColliderId)); + } + } + } + return; + } + _queryPairHelper(node.Left, node.Right); + //_queryPair(node.Left, l, node.SplitIndex); + //_queryPair(node.Right, node.SplitIndex + 1, r); + + //if (node.Left.Box.Intersects(node.Right.Box)) + //{ + // for (int i = l; i <= node.SplitIndex; i++) + // { + // for (int j = node.SplitIndex + 1; j <= r; j++) + // { + // if ((_colliders[i].IsDynamic || _colliders[j].IsDynamic) && + // _colliders[i].BoundingBox.Intersects(_colliders[j].BoundingBox)) + // { + // _pairQueryCache.Add(new KeyValuePair(_colliders[i].ColliderId, + // _colliders[j].ColliderId)); + // } + // } + // } + //} + } + + public List> QueryPairs() + { + _pairQueryCache.Clear(); + if (_colliders.Count == 0) + { + return _pairQueryCache; + } + _clearFlags(Root); + _innerPairQuery(Root); + _queryPair(Root, 0, _colliders.Count - 1); + return _pairQueryCache; + } + + public List QueryRange(in AABB targetRange) + { + List result = new List(); + if (!targetRange.Intersects(Root.Box)) + return result; + + Stack stack = new Stack(); + stack.Push(Root); + + while (stack.Count > 0) + { + var cur = stack.Pop(); + if (!cur.Box.Intersects(targetRange)) + { + continue; + } + if (cur.Colliders.Count != 0) + { + foreach (var collider in cur.Colliders) + { + if (collider.BoundingBox.Intersects(targetRange)) + { + result.Add(collider.ColliderId); + } + } + } + else + { + if (cur.Left.Box.Intersects(targetRange)) + { + stack.Push(cur.Left); + } + if (cur.Right.Box.Intersects(targetRange)) + { + stack.Push(cur.Right); + } + } + } + return result; + } + + private void _getProfilingData(BVHNode node, int layer, List result) + { + if(node == null) return; + result.Add(new ProfilingDataBVH() + { + GridBox = node.Box, + Layer = layer, + }); + + _getProfilingData(node.Left, layer + 1, result); + _getProfilingData(node.Right, layer + 1, result); + } + public List GetProfilingData() + { + List result = new List(); + _getProfilingData(Root, 0, result); + return result; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/Structure/ColliderEntry.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/Structure/ColliderEntry.cs new file mode 100644 index 000000000..76b4c2a68 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/Structure/ColliderEntry.cs @@ -0,0 +1,9 @@ +namespace Everglow.Commons.Physics.PBEngine.Collision.BroadPhase.Structure +{ + public struct ColliderEntry + { + public int ColliderId; + public AABB BoundingBox; + public bool IsDynamic; + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/Structure/CollisionGraph.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/Structure/CollisionGraph.cs new file mode 100644 index 000000000..794010346 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/Structure/CollisionGraph.cs @@ -0,0 +1,60 @@ +namespace Everglow.Commons.Physics.PBEngine.Collision.BroadPhase.Structure +{ + /// + /// 存储碰撞组的碰撞关系的图结构 + /// + public class CollisionGraph + { + public static CollisionGraph DefaultGraph + = new CollisionGraph(new Dictionary>() { { "Default", new List() { "Default" } } }); + + public Dictionary> Graph + { + get; + set; + } + + public CollisionGraph() + { + Graph = new Dictionary>(); + } + + public CollisionGraph(Dictionary> graph) + { + Graph = graph; + } + + public void AddDoubleEdge(string groupA, string groupB) + { + if (groupA == groupB) + { + return; + } + + // Ensure only compare once + if (string.CompareOrdinal(groupA, groupB) < 0) + { + AddSingleEdge(groupA, groupB); + } + else + { + AddSingleEdge(groupB, groupA); + } + } + + public void AddSingleEdge(string groupA, string groupB) + { + if (Graph.ContainsKey(groupA)) + { + Graph[groupA].Add(groupB); + } + else + { + Graph.Add(groupA, new List() + { + groupB, + }); + } + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/Structure/MultiLevelHashGrid.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/Structure/MultiLevelHashGrid.cs new file mode 100644 index 000000000..88df0d935 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/Structure/MultiLevelHashGrid.cs @@ -0,0 +1,431 @@ +using Everglow.Commons.Physics.PBEngine.Collision; +using log4net.Core; +using Microsoft.Xna.Framework; +using Steamworks; +using System; +using System.Collections; +using System.Collections.Generic; +using System.Diagnostics.CodeAnalysis; +using System.IO.Pipes; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Everglow.Commons.Physics.PBEngine.Collision.BroadPhase.Structure +{ + public struct ProfilingData + { + public AABB GridBox; + public int NumObjects; + } + public class MultiLevelHashGrid + { + private class HGrid + { + public List Entries; + + + public HGrid() + { + Entries = new List(); + } + + public List> GetPairs() + { + List> pairs = new List>(); + + for (int i = 0; i < Entries.Count; i++) + { + for (int j = i + 1; j < Entries.Count; j++) + { + if (Entries[i].BoundingBox.Intersects(Entries[j].BoundingBox) + && (Entries[i].IsDynamic || Entries[j].IsDynamic)) + { + // 序号小的在前面，这样判重方便一点 + if (Entries[i].ColliderId < Entries[j].ColliderId) + { + pairs.Add(new KeyValuePair(Entries[i].ColliderId, Entries[j].ColliderId)); + } + else + { + pairs.Add(new KeyValuePair(Entries[j].ColliderId, Entries[i].ColliderId)); + } + } + } + } + return pairs; + } + + public List> GetPairsWith(HGrid grid) + { + List> pairs = new List>(); + + for (int i = 0; i < Entries.Count; i++) + { + for (int j = 0; j < grid.Entries.Count; j++) + { + if (Entries[i].BoundingBox.Intersects(grid.Entries[j].BoundingBox) + && (Entries[i].IsDynamic || grid.Entries[j].IsDynamic)) + { + pairs.Add(new KeyValuePair(Entries[i].ColliderId, grid.Entries[j].ColliderId)); + } + } + } + return pairs; + } + + public List QueryRange(in AABB aabb) + { + List result = new List(); + for (int i = 0; i < Entries.Count; i++) + { + if (Entries[i].BoundingBox.Intersects(aabb)) + { + result.Add(Entries[i].ColliderId); + } + } + return result; + } + } + + private int _maxLevels; + private int _maxOccupiedGridsPerLevel; + private int[] _sizeThisLevel; + private Dictionary[] _multiGrids; + public MultiLevelHashGrid(int maxGridsPerObjectPerLevel, int[] sizesEachLevel) + { + _maxLevels = sizesEachLevel.Length; + _maxOccupiedGridsPerLevel = maxGridsPerObjectPerLevel; + _sizeThisLevel = new int[_maxLevels + 1]; + _sizeThisLevel[0] = 0; + for(int i = 1; i <= _maxLevels; i++) + { + _sizeThisLevel[i] = sizesEachLevel[i - 1]; + } + _multiGrids = new Dictionary[_maxLevels + 1]; + for (int i = 0; i <= _maxLevels; i++) + { + _multiGrids[i] = new Dictionary(); + } + } + + public void AddRange(List entries) + { + foreach (var entry in entries) + { + int level = FindMatchingLevel(entry.BoundingBox); + foreach (var grid in OccupiedRange(entry.BoundingBox, level)) + { + Insert(level, grid, entry); + } + } + } + + public void Add(ColliderEntry entry) + { + int level = FindMatchingLevel(entry.BoundingBox); + foreach (var grid in OccupiedRange(entry.BoundingBox, level)) + { + Insert(level, grid, entry); + } + } + + public List QueryRange(in AABB aabb) + { + List results = new List(1024); + for (int i = 0; i <= _maxLevels; i++) + { + results.AddRange(QueryRange_Level(aabb, i)); + } + return results; + } + + + private List QueryRange_Level(in AABB aabb, int level) + { + List results = new List(); + if (level == 0) + { + if (_multiGrids[0].ContainsKey(new Point(0, 0))) + { + // 和大物体的碰撞 + foreach (var largeEntry in _multiGrids[0][new Point(0, 0)].Entries) + { + if (largeEntry.BoundingBox.Intersects(aabb)) + { + results.Add(largeEntry.ColliderId); + } + } + } + return results; + } + + if (aabb.GetArea() < (double)_sizeThisLevel[level] * _sizeThisLevel[level] * _multiGrids[level].Count) + { + foreach (var grid in OccupiedRange(aabb, level)) + { + if (_multiGrids[level].ContainsKey(grid)) + { + var selfAABB = GetGridAABB(grid, level); + if (selfAABB.CompletelyInside(aabb)) + { + results.AddRange(_multiGrids[level][grid].Entries.Select(x => x.ColliderId).ToList()); + } + else + { + results.AddRange(_multiGrids[level][grid].QueryRange(aabb)); + } + } + } + } + else + { + foreach (var gridpair in _multiGrids[level]) + { + var selfAABB = GetGridAABB(gridpair.Key, level); + if (selfAABB.Intersects(aabb)) + { + if (selfAABB.CompletelyInside(aabb)) + { + results.AddRange(gridpair.Value.Entries.Select(x => x.ColliderId).ToList()); + } + else + { + results.AddRange(gridpair.Value.QueryRange(aabb)); + } + } + } + } + return results; + } + + public List> QueryPairs() + { + List> pairs = new List>(1024); + + for (int i = 0; i <= _maxLevels; i++) + { + foreach (var pair in _multiGrids[i]) + { + pairs.AddRange(pair.Value.GetPairs()); + } + } + + + for (int i = 1; i <= _maxLevels; i++) + { + // 和上层进行相互检测 + for (int j = 0; j < i; j++) + { + if (_multiGrids[j].Count == 0) + { + continue; + } + foreach (var pair in _multiGrids[i]) + { + var rp = new Point(0, 0); + var pos = new Point(pair.Key.X * _sizeThisLevel[i], pair.Key.Y * _sizeThisLevel[i]); + // 获取上层中当前格子对应位置 + if (j != 0) + { + int X = (int)Math.Floor((float)pos.X / _sizeThisLevel[j]); + int Y = (int)Math.Floor((float)pos.Y / _sizeThisLevel[j]); + rp = new Point(X, Y); + } + if (_multiGrids[j].ContainsKey(rp)) + { + pairs.AddRange(pair.Value.GetPairsWith(_multiGrids[j][rp])); + } + } + } + } + return pairs.Distinct().ToList(); + } + + + public List> QueryPairsWith(MultiLevelHashGrid other) + { + + List> pairs = new List>(1024); + // 尺寸相等格子 + for (int i = 0; i <= _maxLevels; i++) + { + if (_multiGrids[i].Count < other._multiGrids[i].Count) + { + foreach (var pair in _multiGrids[i]) + { + if (other._multiGrids[i].ContainsKey(pair.Key)) + { + pairs.AddRange(pair.Value.GetPairsWith(other._multiGrids[i][pair.Key])); + } + } + } + else + { + foreach (var pair in other._multiGrids[i]) + { + if (_multiGrids[i].ContainsKey(pair.Key)) + { + pairs.AddRange(_multiGrids[i][pair.Key].GetPairsWith(pair.Value)); + } + } + } + } + + // 自己尺寸比对方尺寸大 + for (int i = 0; i <= _maxLevels; i++) + { + for (int j = i + 1; j <= _maxLevels; j++) + { + foreach (var pair_other in other._multiGrids[j]) + { + Point rp = new Point(0, 0); + if (i != 0) + { + var pos = new Point(pair_other.Key.X * _sizeThisLevel[j], pair_other.Key.Y * _sizeThisLevel[j]); + // 获取上层中当前格子对应位置 + int X = (int)Math.Floor((float)pos.X / _sizeThisLevel[i]); + int Y = (int)Math.Floor((float)pos.Y / _sizeThisLevel[i]); + rp = new Point(X, Y); + } + if (_multiGrids[i].ContainsKey(rp)) + { + pairs.AddRange(_multiGrids[i][rp].GetPairsWith(pair_other.Value)); + } + } + } + } + + // 自己尺寸比对方尺寸小 + for (int i = 0; i <= _maxLevels; i++) + { + for (int j = i + 1; j <= _maxLevels; j++) + { + foreach (var pair_self in _multiGrids[j]) + { + Point rp = new Point(0, 0); + if (i != 0) + { + var pos = new Point(pair_self.Key.X * _sizeThisLevel[j], pair_self.Key.Y * _sizeThisLevel[j]); + // 获取上层中当前格子对应位置 + int X = (int)Math.Floor((float)pos.X / _sizeThisLevel[i]); + int Y = (int)Math.Floor((float)pos.Y / _sizeThisLevel[i]); + rp = new Point(X, Y); + } + if (other._multiGrids[i].ContainsKey(rp)) + { + pairs.AddRange(pair_self.Value.GetPairsWith(other._multiGrids[i][rp])); + } + } + } + } + return pairs.Distinct().ToList(); + } + + private int FindMatchingLevel(in AABB aabb) + { + int ans = _maxLevels; + int l = 0, r = _maxLevels; + while (l <= r) + { + int mid = (l + r) >> 1; + if (CanFitInThisLevel(aabb, mid)) + { + l = mid + 1; + ans = mid; + } + else + { + r = mid - 1; + } + } + return ans; + } + + private void Insert(int level, Point pos, ColliderEntry entry) + { + if (!_multiGrids[level].ContainsKey(pos)) + { + _multiGrids[level].Add(pos, new HGrid()); + } + + _multiGrids[level][pos].Entries.Add(entry); + } + + private List OccupiedRange(in AABB aabb, int level) + { + if (level == 0) + { + return new List() + { + new Point(0, 0), + }; + } + int minX = (int)Math.Floor(aabb.MinPoint.X / _sizeThisLevel[level]); + int maxX = (int)Math.Floor(aabb.MaxPoint.X / _sizeThisLevel[level]); + int minY = (int)Math.Floor(aabb.MinPoint.Y / _sizeThisLevel[level]); + int maxY = (int)Math.Floor(aabb.MaxPoint.Y / _sizeThisLevel[level]); + + List points = new List(); + for (int x = minX; x <= maxX; x++) + { + for (int y = minY; y <= maxY; y++) + { + points.Add(new Point(x, y)); + } + } + return points; + } + + private bool CanFitInThisLevel(in AABB aabb, int level) + { + if (level == 0) + { + return true; + } + int minX = (int)Math.Floor(aabb.MinPoint.X / _sizeThisLevel[level]); + int maxX = (int)Math.Floor(aabb.MaxPoint.X / _sizeThisLevel[level]); + int minY = (int)Math.Floor(aabb.MinPoint.Y / _sizeThisLevel[level]); + int maxY = (int)Math.Floor(aabb.MaxPoint.Y / _sizeThisLevel[level]); + return Math.Floor((aabb.MaxPoint.X - aabb.MinPoint.X) / _sizeThisLevel[level]) + 1 <= _maxOccupiedGridsPerLevel + && Math.Floor((aabb.MaxPoint.Y - aabb.MinPoint.Y) / _sizeThisLevel[level]) + 1 <= _maxOccupiedGridsPerLevel + && (maxX - minX + 1) * (maxY - minY + 1) <= _maxOccupiedGridsPerLevel; + } + + private AABB GetGridAABB(Point p, int level) + { + if (level == 0) + { + // 0层无限大的AABB + return new AABB() + { + MinPoint = new Vector2(float.NegativeInfinity, float.NegativeInfinity), + MaxPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity) + }; + } + int sz = _sizeThisLevel[level]; + return new AABB() + { + MinPoint = new Vector2(p.X * sz, p.Y * sz), + MaxPoint = new Vector2(p.X * sz + sz, p.Y * sz + sz), + }; + } + + public List GetProfilingData() + { + List result = new List(); + for (int i = 1; i <= _maxLevels; i++) + { + foreach (var pair in _multiGrids[i]) + { + result.Add(new ProfilingData() + { + GridBox = GetGridAABB(pair.Key, i), + NumObjects = pair.Value.Entries.Count + }); + } + } + return result; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/SweepAndPrune.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/SweepAndPrune.cs new file mode 100644 index 000000000..3eae70789 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/BroadPhase/SweepAndPrune.cs @@ -0,0 +1,203 @@ +//using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +//using System; +//using System.Collections.Generic; +//using System.Linq; +//using System.Text; +//using System.Threading.Tasks; + +//namespace Everglow.Commons.Physics.PBEngine.Collision.BroadPhase +//{ +// internal class SweepAndPrune : IBroadPhase +// { +// private struct IntervalPoint +// { +// public float V; +// public bool IsEnd; +// public int ColliderId; +// public bool IsDynamic; +// } + +// private class GroupInfo +// { +// public List IntervalPointsX; +// public List IntervalPointsY; + +// public GroupInfo() +// { +// IntervalPointsX = new List(); +// IntervalPointsY = new List(); +// } +// } + +// private List _colliders; +// private Dictionary _groups; +// public SweepAndPrune(CollisionGraph graph) : base(graph) +// { +// _colliders = new List(); +// _groups = new Dictionary(); +// } +// private bool IsDynamic(int id) +// { +// return _colliders[id].ParentObject.RigidBody.MovementType == MovementType.Dynamic +// || _colliders[id].ParentObject.RigidBody.MovementType == MovementType.Player; +// } + +// public override void Prepare(List objects, float deltaTime) +// { +// _colliders.Clear(); +// _colliders.EnsureCapacity(objects.Count); +// _groups.Clear(); +// for (int i = 0; i < objects.Count; i++) +// { +// var obj = objects[i]; +// _colliders.Add(obj.Collider); +// var box = obj.Collider.GetAABB(deltaTime); +// bool isDynamic = obj.RigidBody.MovementType == MovementType.Dynamic +// || obj.RigidBody.MovementType == MovementType.Player; + +// if (!_groups.ContainsKey(obj.Tag)) +// { +// _groups.Add(obj.Tag, new GroupInfo()); +// } +// _groups[obj.Tag].IntervalPointsX.Add(new IntervalPoint() { ColliderId = i, IsEnd = false, V = box.MinPoint.X, IsDynamic = isDynamic }); +// _groups[obj.Tag].IntervalPointsX.Add(new IntervalPoint() { ColliderId = i, IsEnd = true, V = box.MaxPoint.X, IsDynamic = isDynamic }); +// _groups[obj.Tag].IntervalPointsY.Add(new IntervalPoint() { ColliderId = i, IsEnd = false, V = box.MinPoint.Y, IsDynamic = isDynamic }); +// _groups[obj.Tag].IntervalPointsY.Add(new IntervalPoint() { ColliderId = i, IsEnd = true, V = box.MaxPoint.Y, IsDynamic = isDynamic }); +// } + +// Comparer compFunc = Comparer.Create((a, b) => +// { +// int cmp1 = a.V.CompareTo(b.V); +// if (cmp1 == 0) +// { +// if (a.IsEnd && !b.IsEnd) +// { +// return 1; +// } +// else if (!a.IsEnd && b.IsEnd) +// { +// return -1; +// } +// return 0; +// } +// return cmp1; +// }); +// foreach (var group in _groups) +// { +// group.Value.IntervalPointsX.Sort(compFunc); +// group.Value.IntervalPointsY.Sort(compFunc); +// } +// } + +// public override List> GetCollisionPairs(float deltaTime) +// { +// List> finalPairs = new List>(); +// foreach (var group in _groups) +// { +// GroupInnerDetection(group.Key, finalPairs); +// if (_collisionGraph.Graph.ContainsKey(group.Key)) +// { +// foreach (var dual in _collisionGraph.Graph[group.Key]) +// { +// if (!_groups.ContainsKey(dual)) +// continue; +// GroupOuterDetection(group.Key, dual, finalPairs); +// } +// } +// } +// return finalPairs; +// } + +// private void GroupInnerDetection(string key, List> pairs) +// { +// var xpoints = _groups[key].IntervalPointsX; +// var ypoints = _groups[key].IntervalPointsY; + +// HashSet xSet = new HashSet(); +// HashSet ySet = new HashSet(); +// List> xPairs = new List>(); +// List> yPairs = new List>(); +// foreach (var pt in xpoints) +// { +// if (pt.IsEnd) +// { +// xSet.Remove(pt.ColliderId); +// foreach (var c in xSet) +// { +// if (pt.IsDynamic || IsDynamic(c)) +// { +// // 序号小的在前面，这样判重方便一点 +// if (pt.ColliderId < c) +// { +// xPairs.Add(new KeyValuePair(pt.ColliderId, c)); +// } +// else +// { +// xPairs.Add(new KeyValuePair(c, pt.ColliderId)); +// } +// } +// } +// } +// else +// { +// xSet.Add(pt.ColliderId); +// } +// } + +// foreach (var pt in ypoints) +// { +// if (pt.IsEnd) +// { +// ySet.Remove(pt.ColliderId); +// foreach (var c in ySet) +// { +// if (pt.IsDynamic || IsDynamic(c)) +// { +// // 序号小的在前面，这样判重方便一点 +// if (pt.ColliderId < c) +// { +// yPairs.Add(new KeyValuePair(pt.ColliderId, c)); +// } +// else +// { +// yPairs.Add(new KeyValuePair(c, pt.ColliderId)); +// } +// } +// } +// } +// else +// { +// ySet.Add(pt.ColliderId); +// } +// } + +// xPairs.AddRange(yPairs); + +// xPairs.Sort((a, b) => +// { +// int cmp1 = a.Key.CompareTo(b.Key); +// if (cmp1 == 0) +// { +// return a.Value.CompareTo(b.Value); +// } +// return cmp1; +// }); + +// for(int i = 0; i < xPairs.Count; i++) +// { +// var pt = xPairs[i]; +// if (i == xPairs.Count - 1 || xPairs[i + 1].Key != pt.Key || xPairs[i + 1].Value != pt.Value) +// { +// continue; +// } +// pairs.Add(new KeyValuePair(_colliders[pt.Key], _colliders[pt.Value])); +// i++; +// } +// } + +// public override List GetSingleCollision(AABB aabb, Vector2 velocity, float deltaTime, List targetTags) +// { +// throw new NotImplementedException(); +// } +// } +//} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/Colliders/BoxCollider.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/Colliders/BoxCollider.cs new file mode 100644 index 000000000..2d6cff39e --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/Colliders/BoxCollider.cs @@ -0,0 +1,507 @@ +using Everglow.Commons.Physics.PBEngine.Collision.Shapes; +using Everglow.Commons.Physics.PBEngine.Core; +using Everglow.Commons.Physics.PBEngine.GameInteraction; + +namespace Everglow.Commons.Physics.PBEngine.Collision.Colliders +{ + /// + /// 四边形、盒子碰撞体 + /// + public class BoxCollider : Collider2D + { + public Vector2 Size + { + get => new Vector2(_width, _height); + } + + private float _width; + private float _height; + + public BoxCollider(float width, float height) + { + _width = width; + _height = height; + } + + public override double InertiaTensor(float mass) + { + return mass / 12.0 * (_width * _width + _height * _height); + } + + public override List GetWireFrameWires() + { + List wires = new List + { + new Vector2(-_width / 2, -_height / 2), + new Vector2(_width / 2, -_height / 2), + new Vector2(_width / 2, -_height / 2), + new Vector2(_width / 2, _height / 2), + new Vector2(_width / 2, _height / 2), + new Vector2(-_width / 2, _height / 2), + new Vector2(-_width / 2, _height / 2), + new Vector2(-_width / 2, -_height / 2), + }; + return wires; + } + + public List GetEdges(float dt) + { + List cornerWorldSpace = GetCornerPoints(dt); + List edges = new List(); + for (int i = 0; i < 4; i++) + { + edges.Add(new Edge2D(cornerWorldSpace[i], cornerWorldSpace[(i + 1) % cornerWorldSpace.Count])); + } + return edges; + } + + // public override List GetCollisionEvents(Collider2D other, float deltaTime) + // { + // List events = new List(); + + // Vector2[] localPoints = new Vector2[4] + // { + // new Vector2(-_width / 2, -_height / 2), + // new Vector2(_width / 2, -_height / 2), + // new Vector2(_width / 2, _height / 2), + // new Vector2(-_width / 2, _height / 2) + // }; + // if (other is AABBCollider) + // { + // AABBCollider aabb_other = (AABBCollider)other; + // for (int i = 0; i < 4; i++) + // { + // var curPos = _bindObject.Position + Matrix2x2.CreateRotationMatrix(_bindObject.Rotation).Multiply(localPoints[i]); + // var oldPos = _bindObject.OldPosition + Matrix2x2.CreateRotationMatrix(_bindObject.OldRotation).Multiply(localPoints[i]); + + // foreach (var edge in aabb_other.GetEdges()) + // { + // CollisionEvent2D collision; + // if (edge.Segment_Segment_Collision(oldPos, curPos, out collision)) + // { + // collision.Source = this.ParentObject; + // collision.Target = other.ParentObject; + // collision.LocalOffsetSrc = Matrix2x2.CreateRotationMatrix(_bindObject.Rotation).Multiply(localPoints[i]); + + // events.Add(collision); + // } + // } + // } + // } + // if (events.Count > 1) + // { + // Vector2 averagedLocalPoint = new Vector2(0, 0); + // Vector2 averagedNormal = new Vector2(0, 0); + // float minTime = 1.0f; + // for (int i = 0; i < events.Count; i++) + // { + // averagedLocalPoint += events[i].LocalOffsetSrc; + // averagedNormal += events[i].Normal; + // minTime = Math.Min(minTime, events[i].Time); + // } + // averagedLocalPoint /= events.Count; + // averagedNormal /= events.Count; + // CollisionEvent2D collision = new CollisionEvent2D(); + // collision.Source = this.ParentObject; + // collision.Target = other.ParentObject; + // collision.LocalOffsetSrc = averagedLocalPoint; + // collision.Normal = Vector2.Normalize(averagedNormal); + // collision.Time = minTime; + + // events.Clear(); + // events.Add(collision); + // } + // return events; + // } + public List GetCornerPoints(float dt) + { + List cornerPoints = new List(); + Vector2[] localPoints = new Vector2[4] + { + new Vector2(-_width / 2, -_height / 2), + new Vector2(_width / 2, -_height / 2), + new Vector2(_width / 2, _height / 2), + new Vector2(-_width / 2, _height / 2), + }; + + var M = ParentObject.CachedRotationalMatrix; + for (int i = 0; i < 4; i++) + { + var curPos = ParentObject.Position + M.Multiply(localPoints[i]); + cornerPoints.Add(curPos); + } + return cornerPoints; + } + + public override bool TestCollisionCondition(Collider2D other, float deltaTime, out CollisionInfo info) + { + info = default(CollisionInfo); + if (other is BoxCollider) + { + BoxCollider b = (BoxCollider)other; + List edgesA = GetEdges(deltaTime); + List edgesB = b.GetEdges(deltaTime); + List cornersA = GetCornerPoints(deltaTime); + List cornersB = b.GetCornerPoints(deltaTime); + float depth; + Vector2 normal; + if (GeometryUtils.ConvexPolygonPolygonCollisionInfo(edgesA, cornersA, edgesB, cornersB, out depth, out normal)) + { + info.Source = this.ParentObject; + info.Target = b.ParentObject; + info.Time = deltaTime; + info.Depth = depth; + info.Normal = Vector2.Dot(normal, ParentObject.RigidBody.CentroidWorldSpace - b.ParentObject.RigidBody.CentroidWorldSpace) < 0 ? -normal : normal; + return true; + } + } + else if (other is SphereCollider) + { + SphereCollider b = (SphereCollider)other; + float depth; + Vector2 normal; + bool collided = GeometryUtils.SphereConvexPolygonCollisionInfo(b.ParentObject.Position, b.Radius, GetEdges(deltaTime), + GetCornerPoints(deltaTime), + out depth, out normal); + if (collided) + { + info.Source = this.ParentObject; + info.Target = b.ParentObject; + info.Time = deltaTime; + info.Depth = depth; + info.Normal = Vector2.Dot(normal, ParentObject.RigidBody.CentroidWorldSpace - b.ParentObject.RigidBody.CentroidWorldSpace) < 0 ? -normal : normal; + } + return collided; + } + else if (other is TileCollider) + { + float depth; + Vector2 normal; + if (TileCollisionUtils.GetPolygonTileCollisionInfo(GetAABB(deltaTime), ParentObject.RigidBody.CentroidWorldSpace, + GetEdges(deltaTime), GetCornerPoints(deltaTime), out depth, out normal)) + { + info.Source = this.ParentObject; + info.Target = other.ParentObject; + info.Time = deltaTime; + info.Depth = depth; + info.Normal = normal; + return true; + } + } + else if (other is CapsuleCollider) + { + CapsuleCollider b = (CapsuleCollider)other; + b.GetSegment(deltaTime, out Vector2 A, out Vector2 B); + if (GeometryUtils.CapsuleConvexPolygonCollisionInfo(A, B, b.Radius, GetCornerPoints(deltaTime), + GetEdges(deltaTime), + out float depth, out Vector2 normal)) + { + info.Source = this.ParentObject; + info.Target = b.ParentObject; + info.Time = deltaTime; + info.Depth = depth; + info.Normal = Vector2.Dot(normal, ParentObject.RigidBody.CentroidWorldSpace - b.ParentObject.RigidBody.CentroidWorldSpace) < 0 ? -normal : normal; + return true; + } + } + return false; + } + + public override void GetContactInfo(in CollisionInfo info, float deltaTime, + out List collisionEvents) + { + collisionEvents = new List(); + if (info.Target.Collider is BoxCollider) + { + BoxCollider b = (BoxCollider)info.Target.Collider; + List edgesA = GetEdges(deltaTime); + List edgesB = b.GetEdges(deltaTime); + List cornersA = GetCornerPoints(deltaTime); + List cornersB = b.GetCornerPoints(deltaTime); + var e = new CollisionEvent2D() + { + Time = info.Time, + Source = info.Source, + Target = info.Target, + LocalOffsetSrc = Vector2.Zero, + LocalOffsetTarget = Vector2.Zero, + Normal = info.Normal, + Position = Vector2.Zero, + Depth = info.Depth, + }; + List> contacts = new List>(); + GeometryUtils.ConvexPolygonPolygonContactInfo(edgesA, cornersA, edgesB, cornersB, + ParentObject.RigidBody.CentroidWorldSpace, b.ParentObject.RigidBody.CentroidWorldSpace, + contacts); + foreach (var c in contacts) + { + collisionEvents.Add(new CollisionEvent2D(e) + { + LocalOffsetSrc = c.Key, + LocalOffsetTarget = c.Value, + }); + } + } + else if (info.Target.Collider is SphereCollider) + { + SphereCollider b = (SphereCollider)info.Target.Collider; + var e = new CollisionEvent2D() + { + Time = 0, + Source = info.Source, + Target = info.Target, + LocalOffsetSrc = Vector2.Zero, + LocalOffsetTarget = Vector2.Zero, + Normal = info.Normal, + Position = Vector2.Zero, + Depth = info.Depth, + }; + + List> contacts = new List>(); + GeometryUtils.SphereConvexPolygonContactInfo(b.ParentObject.RigidBody.CentroidWorldSpace, b.Radius, GetEdges(deltaTime), + GetCornerPoints(deltaTime), ParentObject.RigidBody.CentroidWorldSpace, contacts); + foreach (var c in contacts) + { + collisionEvents.Add(new CollisionEvent2D(e) + { + LocalOffsetSrc = c.Value, + LocalOffsetTarget = c.Key, + }); + } + } + else if (info.Target.Collider is CapsuleCollider) + { + CapsuleCollider b = (CapsuleCollider)info.Target.Collider; + var e = new CollisionEvent2D() + { + Time = info.Time, + Source = this.ParentObject, + Target = b.ParentObject, + LocalOffsetSrc = Vector2.Zero, + LocalOffsetTarget = Vector2.Zero, + Normal = info.Normal, + Position = Vector2.Zero, + Depth = info.Depth, + }; + b.GetSegment(deltaTime, out Vector2 A, out Vector2 B); + List> contacts = new List>(); + GeometryUtils.CapsuleConvexPolygonContactInfo(A, B, b.Radius, + GetCornerPoints(deltaTime), GetEdges(deltaTime), ParentObject.RigidBody.CentroidWorldSpace, contacts); + foreach (var c in contacts) + { + collisionEvents.Add(new CollisionEvent2D(e) + { + LocalOffsetSrc = c.Value, + LocalOffsetTarget = c.Key, + }); + } + } + else if (info.Target.Collider is TileCollider) + { + TileCollisionUtils.GetPolygonTileContactInfo(GetAABB(deltaTime), ParentObject.RigidBody.CentroidWorldSpace, + GetEdges(deltaTime), GetCornerPoints(deltaTime), ParentObject, info.Target, deltaTime, collisionEvents); + } + } + + public override AABB GetAABB(float deltaTime) + { + var points = GetCornerPoints(deltaTime); + Vector2 minPoint = points[0]; + Vector2 maxPoint = points[0]; + for (int i = 1; i < points.Count; i++) + { + minPoint.X = Math.Min(points[i].X, minPoint.X); + minPoint.Y = Math.Min(points[i].Y, minPoint.Y); + maxPoint.X = Math.Max(points[i].X, maxPoint.X); + maxPoint.Y = Math.Max(points[i].Y, maxPoint.Y); + } + return new AABB() { MaxPoint = maxPoint, MinPoint = minPoint }; + } + } +} + + +// float maxDepth = -1; +// if (edgeBelongsToA) +// { +// for (int i = 0; i < cornersB.Count; i++) +// { +// if (IsInsideEdge(cornersB[i], keyEdge)) +// { +// float d = Utils.PointDistance2ToSegment(cornersB[i], keyEdge._pA, keyEdge._pB); +// if (d > maxDepth) +// { +// maxDepth = d; +// } +// } +// } +// Debug.Assert(maxDepth >= 0); +// for (int i = 0; i < cornersB.Count; i++) +// { +// if (IsInsideEdge(cornersB[i], keyEdge)) +// { +// float d = Utils.PointDistance2ToSegment(cornersB[i], keyEdge._pA, keyEdge._pB); +// if (d == maxDepth) +// { +// collisionEvents.Add(new CollisionEvent2D() +// { +// Time = 0, +// Source = this.ParentObject, +// Target = b.ParentObject, +// LocalOffsetSrc = Utils.ProjectPointOnSegment(cornersB[i], keyEdge._pA, keyEdge._pB) +// - ParentObject.RigidBody.CentroidWorldSpace, +// LocalOffsetTarget = cornersB[i] - b.ParentObject.RigidBody.CentroidWorldSpace, +// Normal = normal, +// Position = Vector2.Zero, +// Depth = depth +// }); +// } +// } +// } + +// } +// else +// { +// for (int i = 0; i < cornersA.Count; i++) +// { +// if (IsInsideEdge(cornersA[i], keyEdge)) +// { +// float d = Utils.PointDistance2ToSegment(cornersA[i], keyEdge._pA, keyEdge._pB); +// if (d > maxDepth) +// { +// maxDepth = d; +// } +// } +// } +// Debug.Assert(maxDepth >= 0); +// for (int i = 0; i < cornersA.Count; i++) +// { +// if (IsInsideEdge(cornersA[i], keyEdge)) +// { +// float d = Utils.PointDistance2ToSegment(cornersA[i], keyEdge._pA, keyEdge._pB); +// if (d == maxDepth) +// { +// collisionEvents.Add(new CollisionEvent2D() +// { +// Time = 0, +// Source = b.ParentObject, +// Target = this.ParentObject, +// LocalOffsetSrc = Utils.ProjectPointOnSegment(cornersA[i], keyEdge._pA, keyEdge._pB) +// - b.ParentObject.RigidBody.CentroidWorldSpace, +// LocalOffsetTarget = cornersA[i] - ParentObject.RigidBody.CentroidWorldSpace, +// Normal = -normal, +// Position = Vector2.Zero, +// Depth = depth +// }); +// } +// } +// } +// } +// collisionEvents.Add(new CollisionEvent2D() +// { +// Time = 0, +// Source = this.ParentObject, +// Target = b.ParentObject, +// LocalOffsetSrc = curPoint - (b.ParentObject.RigidBody.CentroidWorldSpace + b.ParentObject.RigidBody.LinearVelocity * (float)ans), +// LocalOffsetTarget = curPoint - (ParentObject.RigidBody.CentroidWorldSpace + ParentObject.RigidBody.LinearVelocity * (float)ans), +// Normal = -Vector2.Normalize(Utils.Rotate90(closestEdge._pB - closestEdge._pA)), +// Position = curPoint, +// }); + +// AABBCollider b = (AABBCollider)other; +// double l = 0, r = deltaTime; +// double ans = r; +// while (r - l > deltaTime * 1e-2) +// { +// double midTime = (r + l) / 2.0; +// if (IsCollideWith(b, (float)midTime)) +// { +// ans = Math.Min(ans, midTime); +// r = midTime; +// } +// else +// { +// l = midTime; +// } +// } + +// double t = Math.Max(0, ans); +// var curPointsA = GetCornerPoints((float)ans); +// var curEdgesA = GetEdges((float)ans); +// var curPointsB = b.GetCornerPoints((float)ans); +// var curEdgesB = b.GetEdges((float)ans); + +//// Vertex/Edge collision, A to B +// foreach (var curPoint in curPointsA) +// { +// bool isInside = true; +// Edge2D closestEdge = null; +// float closestDistance = float.PositiveInfinity; +// foreach(var curEdge in curEdgesB) +// { +// if (!IsInsideEdge(curPoint, curEdge)) +// { +// isInside = false; +// break; +// } +// float dist = Utils.PointDistance2ToSegment(curPoint, curEdge._pA, curEdge._pB); +// if (dist < closestDistance) +// { +// closestDistance = dist; +// closestEdge = curEdge; +// } +// } + +// if(isInside) +// { +// collisionEvents.Add(new CollisionEvent2D() +// { +// Time = (float)(t), +// Source = this.ParentObject, +// Target = b.ParentObject, +// LocalOffsetSrc = curPoint - (ParentObject.RigidBody.CentroidWorldSpace + ParentObject.RigidBody.LinearVelocity * (float)ans), +// LocalOffsetTarget = curPoint - (b.ParentObject.RigidBody.CentroidWorldSpace + b.ParentObject.RigidBody.LinearVelocity * (float)ans), +// Normal = -Vector2.Normalize(Utils.Rotate90(closestEdge._pB - closestEdge._pA)), +// Position = curPoint, + +// }); +// } + +// } + +//// Vertex/Edge collision, B to A +// foreach (var curPoint in curPointsB) +// { +// bool isInside = true; +// Edge2D closestEdge = null; +// float closestDistance = float.PositiveInfinity; +// foreach (var curEdge in curEdgesA) +// { +// if (!IsInsideEdge(curPoint, curEdge)) +// { +// isInside = false; +// break; +// } +// float dist = Utils.PointDistance2ToSegment(curPoint, curEdge._pA, curEdge._pB); +// if (dist < closestDistance) +// { +// closestDistance = dist; +// closestEdge = curEdge; +// } +// } + +// if (isInside) +// { +// collisionEvents.Add(new CollisionEvent2D() +// { +// Time = (float)(t), +// Source = b.ParentObject, +// Target = this.ParentObject, +// LocalOffsetSrc = curPoint - (b.ParentObject.RigidBody.CentroidWorldSpace + b.ParentObject.RigidBody.LinearVelocity * (float)ans), +// LocalOffsetTarget = curPoint - (ParentObject.RigidBody.CentroidWorldSpace + ParentObject.RigidBody.LinearVelocity * (float)ans), +// Normal = -Vector2.Normalize(Utils.Rotate90(closestEdge._pB - closestEdge._pA)), +// Position = curPoint, +// }); +// } + +// } diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/Colliders/CapsuleCollider.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/Colliders/CapsuleCollider.cs new file mode 100644 index 000000000..ad52a82c7 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/Colliders/CapsuleCollider.cs @@ -0,0 +1,262 @@ +using Everglow.Commons.Physics.PBEngine.Core; +using Everglow.Commons.Physics.PBEngine.GameInteraction; +using Everglow.Commons.Utilities; +using Terraria; + +namespace Everglow.Commons.Physics.PBEngine.Collision.Colliders +{ + internal class CapsuleCollider : Collider2D + { + private float _length; + private float _radius; + + public float Length + { + get => _length; + set => _length = value; + } + + public float Radius + { + get => _radius; + set => _radius = value; + } + + public CapsuleCollider(float length, float radius) + { + _length = length; + _radius = radius; + } + + public override AABB GetAABB(float deltaTime) + { + var M = Matrix2x2.CreateRotationMatrix(ParentObject.OldRotation + ParentObject.RigidBody.AngularVelocity * deltaTime); + Vector2 P1 = ParentObject.OldPosition + ParentObject.RigidBody.LinearVelocity * deltaTime + + M.Multiply(new Vector2(_length / 2, 0)); + Vector2 P2 = ParentObject.OldPosition + ParentObject.RigidBody.LinearVelocity * deltaTime + + M.Multiply(new Vector2(-_length / 2, 0)); + + if (P1.X > P2.X) + { + (P2.X, P1.X) = (P1.X, P2.X); + } + if (P1.Y > P2.Y) + { + (P2.Y, P1.Y) = (P1.Y, P2.Y); + } + return new AABB() + { + MinPoint = P1 - new Vector2(_radius, _radius), + MaxPoint = P2 + new Vector2(_radius, _radius), + }; + } + + public override void GetContactInfo(in CollisionInfo info, float deltaTime, out List collisionEvents) + { + collisionEvents = new List(); + if (info.Target.Collider is SphereCollider) + { + var b = (SphereCollider)info.Target.Collider; + var e = new CollisionEvent2D() + { + Time = info.Time, + Source = ParentObject, + Target = b.ParentObject, + LocalOffsetSrc = Vector2.Zero, + LocalOffsetTarget = Vector2.Zero, + Normal = info.Normal, + Position = Vector2.Zero, + Depth = info.Depth, + }; + GetSegment(deltaTime, out Vector2 A, out Vector2 B); + var contacts = new List>(); + GeometryUtils.SphereCapsuleContactInfo( + b.ParentObject.RigidBody.CentroidWorldSpace, + b.Radius, A, B, _radius, contacts); + foreach (var c in contacts) + { + collisionEvents.Add(new CollisionEvent2D(e) + { + LocalOffsetSrc = c.Value, + LocalOffsetTarget = c.Key, + }); + } + } + else if (info.Target.Collider is CapsuleCollider) + { + var b = (CapsuleCollider)info.Target.Collider; + var e = new CollisionEvent2D() + { + Time = info.Time, + Source = ParentObject, + Target = b.ParentObject, + LocalOffsetSrc = Vector2.Zero, + LocalOffsetTarget = Vector2.Zero, + Normal = info.Normal, + Position = Vector2.Zero, + Depth = info.Depth, + }; + GetSegment(deltaTime, out Vector2 A, out Vector2 B); + b.GetSegment(deltaTime, out Vector2 A2, out Vector2 B2); + var contacts = new List>(); + GeometryUtils.CapsuleCapsuleContactInfo(A, B, _radius, A2, B2, b._radius, contacts); + foreach (var c in contacts) + { + collisionEvents.Add(new CollisionEvent2D(e) + { + LocalOffsetSrc = c.Key, + LocalOffsetTarget = c.Value, + }); + } + } + else if (info.Target.Collider is BoxCollider) + { + var b = (BoxCollider)info.Target.Collider; + var e = new CollisionEvent2D() + { + Time = info.Time, + Source = ParentObject, + Target = b.ParentObject, + LocalOffsetSrc = Vector2.Zero, + LocalOffsetTarget = Vector2.Zero, + Normal = info.Normal, + Position = Vector2.Zero, + Depth = info.Depth, + }; + GetSegment(deltaTime, out Vector2 A, out Vector2 B); + var contacts = new List>(); + GeometryUtils.CapsuleConvexPolygonContactInfo(A, B, _radius, + b.GetCornerPoints(deltaTime), b.GetEdges(deltaTime), b.ParentObject.RigidBody.CentroidWorldSpace, contacts); + foreach (var c in contacts) + { + collisionEvents.Add(new CollisionEvent2D(e) + { + LocalOffsetSrc = c.Key, + LocalOffsetTarget = c.Value, + }); + } + } + else if (info.Target.Collider is TileCollider) + { + GetSegment(deltaTime, out Vector2 A, out Vector2 B); + GameInteraction.TileCollisionUtils.GetCapsuleTileContactInfo(GetAABB(deltaTime), A, B, _radius, ParentObject, info.Target, deltaTime, collisionEvents); + } + } + + public override List GetWireFrameWires() + { + var lines = new List(); + lines.Add(new Vector2(_length / 2, _radius)); + lines.Add(new Vector2(-_length / 2, _radius)); + + float da = MathHelper.Pi / 32; + var leftCenter = new Vector2(-_length / 2, 0); + for (int i = 0; i < 32; i++) + { + lines.Add(leftCenter + (i * da + MathHelper.PiOver2).ToRotationVector2() * _radius); + lines.Add(leftCenter + ((i + 1) * da + MathHelper.PiOver2).ToRotationVector2() * _radius); + } + lines.Add(new Vector2(-_length / 2, -_radius)); + lines.Add(new Vector2(_length / 2, -_radius)); + var rightCenter = new Vector2(_length / 2, 0); + for (int i = 0; i < 32; i++) + { + lines.Add(rightCenter + (i * da - MathHelper.PiOver2).ToRotationVector2() * _radius); + lines.Add(rightCenter + ((i + 1) * da - MathHelper.PiOver2).ToRotationVector2() * _radius); + } + lines.Add(Vector2.Zero); + lines.Add(new Vector2(_length / 2 + _radius, 0)); + return lines; + } + + public override double InertiaTensor(float mass) + { + float massOfBox = mass * 2 * _radius * _length / (2 * _radius * _length + MathHelper.Pi * _radius * _radius); + float massOfSphere = mass - massOfBox; + float moi_box = massOfBox / 12.0f * (_length * _length + 4 * _radius * _radius); + float moi_sphere = 0.5f * massOfSphere * _radius * _radius; + float d1 = 3.0f / 8.0f * _radius; + float moi_hsphere = 0.5f * moi_sphere - 0.5f * massOfSphere * d1 * d1; + float d2 = _length / 2 + d1; + moi_hsphere += 0.5f * massOfSphere * d2 * d2; + return moi_box + 2 * moi_hsphere; + } + + public void GetSegment(float dt, out Vector2 segA, out Vector2 segB) + { + var M = ParentObject.CachedRotationalMatrix; + var pos = ParentObject.Position; + segA = pos + M.Multiply(new Vector2(_length / 2, 0)); + segB = pos + M.Multiply(new Vector2(-_length / 2, 0)); + } + + public override bool TestCollisionCondition(Collider2D other, float deltaTime, out CollisionInfo info) + { + info = default; + if (other is SphereCollider) + { + var b = (SphereCollider)other; + GetSegment(deltaTime, out Vector2 A, out Vector2 B); + if (GeometryUtils.SphereCapsuleCollisionInfo(b.ParentObject.RigidBody.CentroidWorldSpace, b.Radius, + A, B, _radius, out float depth, out Vector2 normal)) + { + info.Source = ParentObject; + info.Target = b.ParentObject; + info.Time = deltaTime; + info.Depth = depth; + info.Normal = -normal; + return true; + } + } + else if (other is CapsuleCollider) + { + var b = (CapsuleCollider)other; + GetSegment(deltaTime, out Vector2 A, out Vector2 B); + b.GetSegment(deltaTime, out Vector2 A2, out Vector2 B2); + if (GeometryUtils.CapsuleCapsuleCollisionInfo(A, B, _radius, A2, B2, b._radius, + out float depth, out Vector2 normal)) + { + info.Source = ParentObject; + info.Target = b.ParentObject; + info.Time = deltaTime; + info.Depth = depth; + info.Normal = Vector2.Dot(normal, ParentObject.RigidBody.CentroidWorldSpace - b.ParentObject.RigidBody.CentroidWorldSpace) < 0 ? -normal : normal; + return true; + } + } + else if (other is BoxCollider) + { + var b = (BoxCollider)other; + GetSegment(deltaTime, out Vector2 A, out Vector2 B); + if (GeometryUtils.CapsuleConvexPolygonCollisionInfo(A, B, _radius, b.GetCornerPoints(deltaTime), + b.GetEdges(deltaTime), + out float depth, out Vector2 normal)) + { + info.Source = ParentObject; + info.Target = b.ParentObject; + info.Time = deltaTime; + info.Depth = depth; + info.Normal = Vector2.Dot(normal, ParentObject.RigidBody.CentroidWorldSpace - b.ParentObject.RigidBody.CentroidWorldSpace) < 0 ? -normal : normal; + return true; + } + } + else if (other is TileCollider) + { + float depth; + Vector2 normal; + GetSegment(deltaTime, out Vector2 A, out Vector2 B); + if (GameInteraction.TileCollisionUtils.GetCapsuleTileCollisionInfo(GetAABB(deltaTime), A, B, _radius, out depth, out normal)) + { + info.Source = ParentObject; + info.Target = other.ParentObject; + info.Time = deltaTime; + info.Depth = depth; + info.Normal = normal; + return true; + } + } + + return false; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/Colliders/Collider2D.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/Colliders/Collider2D.cs new file mode 100644 index 000000000..91a8b3333 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/Colliders/Collider2D.cs @@ -0,0 +1,35 @@ +using Everglow.Commons.Physics.PBEngine.Core; + +namespace Everglow.Commons.Physics.PBEngine.Collision.Colliders +{ + /// + /// 碰撞组件的基类 + /// + public abstract class Collider2D + { + public PhysicsObject ParentObject + { + get => _bindObject; + set => _bindObject = value; + } + + protected PhysicsObject _bindObject; + + public abstract double InertiaTensor(float mass); + + public abstract List GetWireFrameWires(); + + // public abstract List GetCollisionEvents(Collider2D other, float deltaTime); + public abstract bool TestCollisionCondition(Collider2D other, float deltaTime, out CollisionInfo info); + + public abstract void GetContactInfo(in CollisionInfo info, float deltaTime, + out List collisionEvents); + + // public abstract List GetContactInfo(CollisionEvent2D e, float deltaTime); + public abstract AABB GetAABB(float deltaTime); + + public Collider2D() + { + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/Colliders/SphereCollider.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/Colliders/SphereCollider.cs new file mode 100644 index 000000000..d2c668025 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/Colliders/SphereCollider.cs @@ -0,0 +1,242 @@ +using Everglow.Commons.Physics.PBEngine.Core; +using Everglow.Commons.Physics.PBEngine.GameInteraction; +using Terraria; + +namespace Everglow.Commons.Physics.PBEngine.Collision.Colliders +{ + /// + /// 圆形碰撞体 + /// + public class SphereCollider : Collider2D + { + public float Radius + { + get => _radius; set => _radius = value; + } + + private float _radius; + + public SphereCollider(float radius) + { + _radius = radius; + } + + public override AABB GetAABB(float deltaTime) + { + var center = ParentObject.OldPosition + ParentObject.RigidBody.LinearVelocity * deltaTime; + return new AABB() + { + MinPoint = center - new Vector2(_radius, _radius), + MaxPoint = center + new Vector2(_radius, _radius), + }; + } + + public override void GetContactInfo(in CollisionInfo info, float deltaTime, out List collisionEvents) + { + collisionEvents = new List(); + if (info.Target.Collider is SphereCollider) + { + SphereCollider b = (SphereCollider)info.Target.Collider; + var e = new CollisionEvent2D() + { + Time = info.Time, + Source = this.ParentObject, + Target = b.ParentObject, + LocalOffsetSrc = Vector2.Zero, + LocalOffsetTarget = Vector2.Zero, + Normal = info.Normal, + Position = Vector2.Zero, + Depth = info.Depth, + }; + + var unit = (ParentObject.RigidBody.CentroidWorldSpace - b.ParentObject.RigidBody.CentroidWorldSpace).SafeNormalize(Vector2.Zero); + collisionEvents.Add(new CollisionEvent2D(e) + { + LocalOffsetSrc = -unit * _radius, + LocalOffsetTarget = unit * b.Radius, + }); + } + else if (info.Target.Collider is BoxCollider) + { + BoxCollider b = (BoxCollider)info.Target.Collider; + var e = new CollisionEvent2D() + { + Time = info.Time, + Source = info.Source, + Target = info.Target, + LocalOffsetSrc = Vector2.Zero, + LocalOffsetTarget = Vector2.Zero, + Normal = Vector2.Dot(info.Normal, ParentObject.RigidBody.CentroidWorldSpace - b.ParentObject.RigidBody.CentroidWorldSpace) < 0 ? -info.Normal : info.Normal, + Position = Vector2.Zero, + Depth = info.Depth, + }; + + List> contacts = new List>(); + GeometryUtils.SphereConvexPolygonContactInfo(ParentObject.RigidBody.CentroidWorldSpace, _radius, b.GetEdges(deltaTime), + b.GetCornerPoints(deltaTime), b.ParentObject.RigidBody.CentroidWorldSpace, contacts); + foreach (var c in contacts) + { + collisionEvents.Add(new CollisionEvent2D(e) + { + LocalOffsetSrc = c.Key, + LocalOffsetTarget = c.Value, + }); + } + } + else if (info.Target.Collider is TileCollider) + { + TileCollisionUtils.GetSphereTileContactInfo(ParentObject.RigidBody.CentroidWorldSpace, _radius, + ParentObject, info.Target, deltaTime, collisionEvents); + } + else if (info.Target.Collider is CapsuleCollider) + { + CapsuleCollider b = (CapsuleCollider)info.Target.Collider; + var e = new CollisionEvent2D() + { + Time = info.Time, + Source = this.ParentObject, + Target = b.ParentObject, + LocalOffsetSrc = Vector2.Zero, + LocalOffsetTarget = Vector2.Zero, + Normal = info.Normal, + Position = Vector2.Zero, + Depth = info.Depth, + }; + b.GetSegment(deltaTime, out Vector2 A, out Vector2 B); + List> contacts = new List>(); + GeometryUtils.SphereCapsuleContactInfo( + ParentObject.RigidBody.CentroidWorldSpace, + _radius, A, B, b.Radius, contacts); + foreach (var c in contacts) + { + collisionEvents.Add(new CollisionEvent2D(e) + { + LocalOffsetSrc = c.Key, + LocalOffsetTarget = c.Value, + }); + } + } + } + + // public override List GetContactInfo(CollisionEvent2D e, float deltaTime) + // { + // List events = new List(); + // if (e.Target.Collider is SphereCollider) + // { + // SphereCollider b = (SphereCollider)e.Target.Collider; + // var unit = (ParentObject.RigidBody.CentroidWorldSpace - b.ParentObject.RigidBody.CentroidWorldSpace).SafeNormalize(Vector2.Zero); + // events.Add(new CollisionEvent2D(e) + // { + // LocalOffsetSrc = -unit * _radius, + // LocalOffsetTarget = unit * b.Radius, + // }); + // } + // else if (e.Target.Collider is BoxCollider) + // { + // BoxCollider b = (BoxCollider)e.Target.Collider; + // List edgesB = b.GetEdges(deltaTime); + // List cornersB = b.GetCornerPoints(deltaTime); + // List> contacts = new List>(); + // Utils.CirclePolygonContactInfo(ParentObject.RigidBody.CentroidWorldSpace, _radius, edgesB, + // cornersB, b.ParentObject.RigidBody.CentroidWorldSpace, contacts); + // foreach (var c in contacts) + // { + // events.Add(new CollisionEvent2D(e) + // { + // LocalOffsetSrc = c.Key, + // LocalOffsetTarget = c.Value, + // }); + // } + // } + // else if (e.Target.Collider is TileCollider) + // { + // TileCollisionUtils.GetSphereTileContactInfo(ParentObject.RigidBody.CentroidWorldSpace, _radius, e, events); + // } + // return events; + // } + public override List GetWireFrameWires() + { + List lines = new List(); + float da = MathHelper.TwoPi / 64; + for (int i = 0; i < 64; i++) + { + lines.Add((i * da).ToRotationVector2() * _radius); + lines.Add(((i + 1) * da).ToRotationVector2() * _radius); + } + lines.Add(Vector2.Zero); + lines.Add(ParentObject.CachedRotationalMatrix.Multiply(new Vector2(_radius, 0))); + return lines; + } + + public override double InertiaTensor(float mass) + { + return 0.5 * mass * _radius * _radius; + } + + public override bool TestCollisionCondition(Collider2D other, float deltaTime, out CollisionInfo info) + { + info = default(CollisionInfo); + if (other is SphereCollider) + { + SphereCollider b = (SphereCollider)other; + float d = Vector2.DistanceSquared(ParentObject.RigidBody.CentroidWorldSpace, b.ParentObject.RigidBody.CentroidWorldSpace); + if (d < (_radius + b._radius) * (_radius + b._radius)) + { + info.Source = this.ParentObject; + info.Target = b.ParentObject; + info.Time = deltaTime; + info.Depth = _radius + b._radius - (float)Math.Sqrt(d); + info.Normal = (ParentObject.RigidBody.CentroidWorldSpace - b.ParentObject.RigidBody.CentroidWorldSpace).SafeNormalize(Vector2.One); + return true; + } + } + else if (other is BoxCollider) + { + BoxCollider b = (BoxCollider)other; + float depth; + Vector2 normal; + if (GeometryUtils.SphereConvexPolygonCollisionInfo(ParentObject.RigidBody.CentroidWorldSpace, _radius, + b.GetEdges(deltaTime), b.GetCornerPoints(deltaTime), out depth, out normal)) + { + info.Source = this.ParentObject; + info.Target = b.ParentObject; + info.Time = deltaTime; + info.Depth = depth; + info.Normal = Vector2.Dot(normal, ParentObject.RigidBody.CentroidWorldSpace - b.ParentObject.RigidBody.CentroidWorldSpace) < 0 ? -normal : normal; + return true; + } + } + else if (other is TileCollider) + { + float depth; + Vector2 normal; + if (TileCollisionUtils.GetSphereTileCollisionInfo(ParentObject.RigidBody.CentroidWorldSpace, _radius, + ParentObject, info.Target, out depth, out normal)) + { + info.Source = this.ParentObject; + info.Target = other.ParentObject; + info.Time = deltaTime; + info.Depth = depth; + info.Normal = normal; + return true; + } + } + else if (other is CapsuleCollider) + { + CapsuleCollider b = (CapsuleCollider)other; + b.GetSegment(deltaTime, out Vector2 A, out Vector2 B); + if (GeometryUtils.SphereCapsuleCollisionInfo(ParentObject.RigidBody.CentroidWorldSpace, Radius, + A, B, b.Radius, out float depth, out Vector2 normal)) + { + info.Source = this.ParentObject; + info.Target = b.ParentObject; + info.Time = deltaTime; + info.Depth = depth; + info.Normal = normal; + return true; + } + } + return false; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/CollisionEvent2D.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/CollisionEvent2D.cs new file mode 100644 index 000000000..735daf518 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/CollisionEvent2D.cs @@ -0,0 +1,68 @@ +using Everglow.Commons.Physics.PBEngine.Core; +using Microsoft.CodeAnalysis.CSharp.Syntax; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Everglow.Commons.Physics.PBEngine.Collision +{ + /// + /// 储存碰撞事件的对象，包括接触点信息 + /// + public class CollisionEvent2D + { + public PhysicsObject Source; + public PhysicsObject Target; + public float Time; + public float Depth; + public Vector2 Position; + public Vector2 Normal; + public Vector2 Offset; + + /// + /// From centroid of source object to hit point + /// + public Vector2 LocalOffsetSrc; + + /// + /// From centroid of target object to hit point + /// + public Vector2 LocalOffsetTarget; + + public float NormalVelOld; + public Vector2 TangentDir; + + public CollisionEvent2D(CollisionEvent2D e) + { + this.Source = e.Source; + this.Target = e.Target; + this.Time = e.Time; + this.Depth = e.Depth; + this.Position = e.Position; + this.Normal = e.Normal; + this.Offset = e.Offset; + this.NormalVelOld = e.NormalVelOld; + } + + public CollisionEvent2D() + { + } + + public CollisionEvent2D CreateMirrorEvent() + { + return new CollisionEvent2D() + { + Source = this.Target, + Target = this.Source, + Time = this.Time, + Position = this.Position, + Normal = -this.Normal, + Offset = -this.Offset, + LocalOffsetSrc = this.LocalOffsetTarget, + LocalOffsetTarget = this.LocalOffsetSrc + }; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/CollisionInfo.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/CollisionInfo.cs new file mode 100644 index 000000000..aa1db2327 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/CollisionInfo.cs @@ -0,0 +1,21 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Everglow.Commons.Physics.PBEngine.Core; + +namespace Everglow.Commons.Physics.PBEngine.Collision +{ + /// + /// 储存碰撞事件的对象，包括穿透深度和挤出方向，不包括接触点 + /// + public struct CollisionInfo + { + public PhysicsObject Source; + public PhysicsObject Target; + public float Time; + public float Depth; + public Vector2 Normal; + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/Shapes/AABB.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/Shapes/AABB.cs new file mode 100644 index 000000000..e3813cd2c --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/Shapes/AABB.cs @@ -0,0 +1,71 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Everglow.Commons.Physics.PBEngine.Collision +{ + public struct AABB + { + public Vector2 MinPoint; + public Vector2 MaxPoint; + + public AABB() + { + MinPoint = new Vector2(float.PositiveInfinity); + MaxPoint = new Vector2(float.NegativeInfinity); + } + + public AABB(Vector2 minP, Vector2 maxP) + { + MinPoint = minP; + MaxPoint = maxP; + } + + public Vector2 Center + { + get => (MaxPoint + MinPoint) / 2; + } + + public bool Intersects(in AABB other) + { + return Math.Max(MinPoint.X, other.MinPoint.X) <= Math.Min(MaxPoint.X, other.MaxPoint.X) + && Math.Max(MinPoint.Y, other.MinPoint.Y) <= Math.Min(MaxPoint.Y, other.MaxPoint.Y); + } + + public AABB Move(Vector2 v) + { + return new AABB() { MinPoint = MinPoint + v, MaxPoint = MaxPoint + v }; + } + + public AABB Merge(in AABB other) + { + return new AABB() { MinPoint = Vector2.Min(MinPoint, other.MinPoint), MaxPoint = Vector2.Max(MaxPoint, other.MaxPoint) }; + } + + public void MergeWith(in AABB other) + { + MinPoint = Vector2.Min(MinPoint, other.MinPoint); + MaxPoint = Vector2.Max(MaxPoint, other.MaxPoint); + } + + public double GetArea() + { + double w = MaxPoint.X - MinPoint.X; + double h = MaxPoint.Y - MinPoint.Y; + return w * h; + } + + public bool CompletelyInside(in AABB other) + { + return other.MinPoint.X <= MinPoint.X && other.MaxPoint.X >= MaxPoint.X + && other.MinPoint.Y <= MinPoint.Y && other.MaxPoint.Y >= MaxPoint.Y; + } + + public AABB EnLarge(float size) + { + return new AABB() { MinPoint = MinPoint - new Vector2(size), MaxPoint = MaxPoint + new Vector2(size) }; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Collision/Shapes/Edge2D.cs b/Sources/Everglow.Function/Physics/PBEngine/Collision/Shapes/Edge2D.cs new file mode 100644 index 000000000..b09096849 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Collision/Shapes/Edge2D.cs @@ -0,0 +1,67 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Everglow.Commons.Physics.PBEngine.Core; +using Everglow.Commons.Utilities; +using Terraria; + +namespace Everglow.Commons.Physics.PBEngine.Collision.Shapes +{ + public class Edge2D + { + public Vector2 _pA, _pB; + public Edge2D() + { + + } + + public Edge2D(Vector2 A, Vector2 B) + { + _pA = A; + _pB = B; + } + + public Vector2 GetNormal() + { + return -GeometryUtils.Rotate90((_pB - _pA).SafeNormalize(Vector2.Zero)); + } + + public bool Segment_Segment_Collision(Vector2 a, Vector2 b, out CollisionEvent2D collisionEvent) + { + var M = new Matrix2x2 + { + [0, 0] = _pB.X - _pA.X, + [0, 1] = -(b.X - a.X), + [1, 0] = _pB.Y - _pA.Y, + [1, 1] = -(b.Y - a.Y), + }; + + double det = M.Determinant(); + if (Math.Abs(det) < 1e-7) + { + collisionEvent = new CollisionEvent2D(); + return false; // parallel lines + } + else + { + + Vector2 st = M.Adjoint().Multiply(a - _pA) / (float)det; + if (st.X >= 0 && st.X <= 1.0f && st.Y >= 0 && st.Y <= 1.0f) + { + collisionEvent = new CollisionEvent2D + { + Time = st.Y, + Position = _pA + (_pB - _pA) * st.X, + Normal = -GeometryUtils.Rotate90((_pB - _pA).SafeNormalize(Vector2.Zero)), + Offset = Vector2.Zero + }; + return true; + } + collisionEvent = new CollisionEvent2D(); + return false; // intersection is outside of the line segments + } + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Constraints/Constraint.cs b/Sources/Everglow.Function/Physics/PBEngine/Constraints/Constraint.cs new file mode 100644 index 000000000..16722cc05 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Constraints/Constraint.cs @@ -0,0 +1,16 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Everglow.Commons.Physics.PBEngine.Constraints +{ + public abstract class Constraint + { + public abstract void Apply(float deltaTime); + public abstract void ApplyForce(float deltaTime); + + public abstract List<(Vector2, Color)> GetDrawMesh(); + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Constraints/JointConstraint.cs b/Sources/Everglow.Function/Physics/PBEngine/Constraints/JointConstraint.cs new file mode 100644 index 000000000..ae2ae980e --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Constraints/JointConstraint.cs @@ -0,0 +1,69 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Everglow.Commons.Physics.PBEngine.Core; +using Terraria; + +namespace Everglow.Commons.Physics.PBEngine.Constraints +{ + /// + /// TODO: 待定约束 + /// + public class JointConstraint : Constraint + { + private PhysicsObject _objA; + private PhysicsObject _objB; + private Vector2 _localPosA; + private Vector2 _localPosB; + public JointConstraint(PhysicsObject A, PhysicsObject B, Vector2 localPos1, Vector2 localPos2) + { + _objA = A; + _objB = B; + _localPosA = localPos1; + _localPosB = localPos2; + } + + public override void Apply(float deltaTime) + { + var pA = _objA.LocalToWorldPos(_localPosA); + // var pB = _objB.LocalToWorldPos(_localPosB); + float w = _objA.RigidBody.InvMass + _objB.RigidBody.InvMass; + float weightA = _objA.RigidBody.InvMass / w; + var newCenter = pA; + var dirA = (newCenter - _objA.Position).SafeNormalize(Vector2.One); + var dirB = (newCenter - _objB.Position).SafeNormalize(Vector2.One); + + float oldRotA = MathHelper.WrapAngle(_objA.Rotation); + float oldRotB = MathHelper.WrapAngle(_objB.Rotation); + //_objA.Rotation = (float)(Math.Atan2(dirA.Y, dirA.X) - Math.Atan2(_localPosA.Y, _localPosA.X)); + //_objB.Rotation = (float)(Math.Atan2(dirB.Y, dirB.X) - Math.Atan2(_localPosB.Y, _localPosB.X)); + + Vector2 addA = newCenter - _objA.LocalToWorldPos(_localPosA); + Vector2 addB = newCenter - _objB.LocalToWorldPos(_localPosB); + Vector2 oldPosB = _objB.Position; + _objA.Position += addA; + _objB.Position = newCenter - dirB * 200; + + _objA.RigidBody.LinearVelocity = (_objA.Position - _objA.OldPosition) / deltaTime; + _objB.RigidBody.LinearVelocity = (_objB.Position - _objB.OldPosition) / deltaTime; + //_objA.RigidBody.AngularVelocity = MathHelper.WrapAngle(_objA.Rotation - _objA.OldRotation) / deltaTime; + //_objB.RigidBody.AngularVelocity = MathHelper.WrapAngle(_objB.Rotation - _objB.OldRotation) / deltaTime; + } + + public override void ApplyForce(float deltaTime) + { + throw new NotImplementedException(); + } + + public override List<(Vector2, Color)> GetDrawMesh() + { + var drawMesh = new List<(Vector2, Color)>(); + drawMesh.Add((_objA.LocalToWorldPos(_localPosA), Color.Green)); + drawMesh.Add((_objB.RigidBody.CentroidWorldSpace, Color.Green)); + + return drawMesh; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Constraints/SpringConstraint.cs b/Sources/Everglow.Function/Physics/PBEngine/Constraints/SpringConstraint.cs new file mode 100644 index 000000000..afcb48fb1 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Constraints/SpringConstraint.cs @@ -0,0 +1,178 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Everglow.Commons.Physics.PBEngine.Core; +using Everglow.Commons.Utilities; +using Terraria; + +namespace Everglow.Commons.Physics.PBEngine.Constraints +{ + /// + /// 弹簧约束 + /// + public class SpringConstraint : Constraint + { + private PhysicsObject _objA; + private PhysicsObject _objB; + private float _compliance; + private float _restLength; + private double _lambda; + private Vector2 _localPosA; + private Vector2 _localPosB; + + public SpringConstraint(PhysicsObject A, PhysicsObject B, float elasticity, float restLength) + { + _objA = A; + _objB = B; + _compliance = 1 / elasticity; + _restLength = restLength; + _lambda = 0; + _localPosA = Vector2.Zero; + _localPosB = Vector2.Zero; + } + + public SpringConstraint(PhysicsObject A, PhysicsObject B, float elasticity, float restLength, + Vector2 localPosA, + Vector2 localPosB) : this(A, B, elasticity, restLength) + { + _localPosA = localPosA; + _localPosB = localPosB; + } + + public static double CalculateShortestAngleDifference(double angle1, double angle2) + { + double difference = angle2 - angle1; + difference = (difference + Math.PI) % (2 * Math.PI) - Math.PI; + + // Adjust the difference to the shortest rotating radians + if (difference < -Math.PI) + { + difference += 2 * Math.PI; + } + else if (difference >= Math.PI) + { + difference -= 2 * Math.PI; + } + + return difference; + } + + public override void Apply(float deltaTime) + { + //var pA = _objA.LocalToWorldPos(_localPosA); + //var pB = _objB.LocalToWorldPos(_localPosB); + //var ra = Matrix2x2.CreateRotationMatrix(_objA.Rotation).Multiply(_localPosA); + //var rb = Matrix2x2.CreateRotationMatrix(_objB.Rotation).Multiply(_localPosB); + //var L = (pB - pA).Length(); + //var unit = (pB - pA).SafeNormalize(Vector2.Zero); + //float C = L - _restLength; + //if (C == 0) + //{ + // return; + //} + + //double rAdotN = Vector2.Dot(GeometryUtils.Rotate90(ra), -(pB - pA)); + //double rBdotN = Vector2.Dot(GeometryUtils.Rotate90(rb), (pB - pA)); + //double R1 = rAdotN * rAdotN * _objA.RigidBody.GlobalInverseInertiaTensor; + //double R2 = rBdotN * rBdotN * _objB.RigidBody.GlobalInverseInertiaTensor; + //float effMassA = (float)(_objA.RigidBody.InvMass + R1); + //float effMassB = (float)(_objB.RigidBody.InvMass + R2); + //float alpha = _compliance / deltaTime / deltaTime; + //double d_lambda = (-C - alpha * _lambda) / (effMassA + effMassB + alpha); + //_lambda = _lambda + d_lambda; + + ////var posA = pA + (float)(_lambda * effMassA ) * -unit ; + ////var posB = pB + (float)(_lambda * effMassB) * unit; + ////float rotA_p = (pA - _objA.RigidBody.CentroidWorldSpace).ToRotation() - _localPosA.ToRotation(); + ////float rotA = (float)CalculateShortestAngleDifference((pA - _objA.RigidBody.CentroidWorldSpace).ToRotation(), + //// (pB - _objA.RigidBody.CentroidWorldSpace).ToRotation()); + ////float rotB_p = (pB - _objB.RigidBody.CentroidWorldSpace).ToRotation() - _localPosB.ToRotation(); + ////float rotB = (float)CalculateShortestAngleDifference((pB - _objB.RigidBody.CentroidWorldSpace).ToRotation(), + //// (posB - _objB.RigidBody.CentroidWorldSpace).ToRotation()); + + //if (_objB.RigidBody.GlobalInverseInertiaTensor != 0) + //{ + // _objB.RigidBody.AddImpluseImmediate((float)(d_lambda ) * (pB - pA), rb); + // //_objB.Rotation += _objB.RigidBody.AngularVelocity * deltaTime; + // //_objB.RigidBody.CentroidWorldSpace += _objB.RigidBody.LinearVelocity * deltaTime; + // //_objB.RigidBody.LinearVelocity = (_objB.RigidBody.CentroidWorldSpace - _objB.OldPosition) / deltaTime; + // //_objB.RigidBody.AngularVelocity = (float)CalculateShortestAngleDifference(_objB.OldRotation, _objB.Rotation) / deltaTime; + //} + //if (_objA.RigidBody.GlobalInverseInertiaTensor != 0) + //{ + // _objA.RigidBody.AddImpluseImmediate((float)(d_lambda) * -(pB - pA), ra); + // //_objA.Rotation += _objA.RigidBody.AngularVelocity * deltaTime; + // //_objA.RigidBody.CentroidWorldSpace += _objA.RigidBody.LinearVelocity * deltaTime; + // //_objA.Rotation += rotA; + // //_objA.RigidBody.CentroidWorldSpace += (posA - _objA.LocalToWorldPos(_localPosA)); + // //_objA.RigidBody.LinearVelocity = (_objA.RigidBody.CentroidWorldSpace - _objA.OldPosition) / deltaTime; + // //_objA.RigidBody.AngularVelocity = (float)CalculateShortestAngleDifference(_objA.OldRotation, _objA.Rotation) / deltaTime; + //} + + + + + + //var ra = Matrix2x2.CreateRotationMatrix(_objA.Rotation).Multiply(_localPosA); + //var rb = Matrix2x2.CreateRotationMatrix(_objB.Rotation).Multiply(_localPosB); + //var da = (_lambda * _objA.RigidBody.InvMass * -unit).SafeNormalize(Vector2.Zero); + //var db = (_lambda * _objB.RigidBody.InvMass * unit).SafeNormalize(Vector2.Zero); + //var oldVelA = Vector2.Dot(_objA.RigidBody.LinearVelocity + GeometryUtils.AnuglarVelocityToLinearVelocity(ra, _objA.RigidBody.AngularVelocity), da); + //var oldVelB = Vector2.Dot(_objB.RigidBody.LinearVelocity + GeometryUtils.AnuglarVelocityToLinearVelocity(rb, _objB.RigidBody.AngularVelocity), db); + + //double rAdotN = Vector2.Dot(GeometryUtils.Rotate90(ra), da); + //double rBdotN = Vector2.Dot(GeometryUtils.Rotate90(rb), db); + //double R1 = rAdotN * rAdotN * _objA.RigidBody.GlobalInverseInertiaTensor; + //double R2 = rBdotN * rBdotN * _objB.RigidBody.GlobalInverseInertiaTensor; + //var impluseA = ((curPosA - pA).Length() / deltaTime - oldVelA) / (_objA.RigidBody.InvMass + (float)R1); + //var impluseB = ((curPosB - pB).Length() / deltaTime - oldVelB) / (_objB.RigidBody.InvMass + (float)R2); + + //if (_objA.RigidBody.InvMass != 0) + // { + // // _objA.RigidBody.AddImpluseImmediate(impluseA * da, ra); + // _objA.Position += _objA.RigidBody.LinearVelocity * deltaTime; + // _objA.Rotation += _objA.RigidBody.AngularVelocity * deltaTime; + + // _objA.RigidBody.LinearVelocity = (_objA.Position - _objA.OldPosition) / deltaTime; + //} + // if (_objB.RigidBody.InvMass != 0) + // { + // _objB.RigidBody.AddImpluseImmediate(impluseB * db, rb); + //_objB.Position += _objB.RigidBody.LinearVelocity * deltaTime; + //_objB.Rotation += _objB.RigidBody.AngularVelocity * deltaTime; + //var test = _objB.LocalToWorldPos(_localPosB); + //if (true) + // ; + + //_objB.RigidBody.LinearVelocity = (_objB.Position - _objB.OldPosition) / deltaTime; + //_objB.RigidBody.AngularVelocity = (_objB.Rotation - _objB.OldRotation) / deltaTime; + // } + //_objA.RigidBody.AngularVelocity = Utils.Cross(_localPosA, _objA.RigidBody.LinearVelocity); + //_objB.RigidBody.AngularVelocity = Utils.Cross(_localPosB, _objB.RigidBody.LinearVelocity); + } + + public override void ApplyForce(float deltaTime) + { + var pA = _objA.LocalToWorldPos(_localPosA); + var pB = _objB.LocalToWorldPos(_localPosB); + var L = (pB - pA).Length(); + var unit = (pB - pA).SafeNormalize(Vector2.Zero); + float C = L - _restLength; + var ra = _objA.CachedRotationalMatrix.Multiply(_localPosA); + var rb = _objB.CachedRotationalMatrix.Multiply(_localPosB); + _objB.RigidBody.AddForce(C * -unit / _compliance, rb); + _objA.RigidBody.AddForce(C * unit / _compliance, ra); + } + + public override List<(Vector2, Color)> GetDrawMesh() + { + var drawMesh = new List<(Vector2, Color)>(); + drawMesh.Add((_objA.LocalToWorldPos(_localPosA), Color.Green)); + drawMesh.Add((_objB.LocalToWorldPos(_localPosB), Color.Green)); + + return drawMesh; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Core/GeometryUtils.cs b/Sources/Everglow.Function/Physics/PBEngine/Core/GeometryUtils.cs new file mode 100644 index 000000000..b317f418b --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Core/GeometryUtils.cs @@ -0,0 +1,735 @@ +using Everglow.Commons.Physics.PBEngine.Collision; +using Everglow.Commons.Physics.PBEngine.Collision.Shapes; +using Everglow.Commons.Utilities; +using System; +using System.Collections; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using Terraria; + +namespace Everglow.Commons.Physics.PBEngine.Core +{ + public static class GeometryUtils + { + + public static AABB ToAABBPhysSpace(this Rectangle rect) + { + return new AABB() + { + MinPoint = new Vector2(rect.X, -(rect.Y + rect.Height)), + MaxPoint = new Vector2(rect.X + rect.Width, -rect.Y) + }; + } + + public static bool ApproxEqual(float a, float b) + { + return Math.Abs(a - b) < 1e-6; + } + /// + /// Compute ab^T + /// + /// + /// + /// + public static Matrix3x3 OuterProduct(Vector3 a, Vector3 b) + { + return new Matrix3x3 + { + [0, 0] = a.X * b.X, + [0, 1] = a.X * b.Y, + [0, 2] = a.X * b.Z, + [1, 0] = a.Y * b.X, + [1, 1] = a.Y * b.Y, + [1, 2] = a.Y * b.Z, + [2, 0] = a.Z * b.X, + [2, 1] = a.Z * b.Y, + [2, 2] = a.Z * b.Z + }; + } + + /// + /// Compute ab^T + /// + /// + /// + /// + public static Matrix2x2 OuterProduct(Vector2 a, Vector2 b) + { + return new Matrix2x2 + { + [0, 0] = a.X * b.X, + [0, 1] = a.X * b.Y, + [1, 0] = a.Y * b.X, + [1, 1] = a.Y * b.Y, + }; + } + + + public static float Cross(this Vector2 a, Vector2 b) + { + return a.X * b.Y - a.Y * b.X; + } + + + public static void Blit(Texture2D source, RenderTarget2D dest, Effect effect, string pass) + { + var sb = Main.spriteBatch; + var gd = Main.graphics.GraphicsDevice; + gd.SetRenderTarget(dest); + gd.Clear(Color.Transparent); + if (effect != null) + { + sb.Begin(SpriteSortMode.Immediate, BlendState.Opaque); + effect.CurrentTechnique.Passes[pass].Apply(); + sb.Draw(source, dest.Bounds, Color.White); + sb.End(); + } + else + { + sb.Begin(); + sb.Draw(source, dest.Bounds, Color.White); + sb.End(); + } + + } + + /// + /// The linear velocity corresponding to given angular velocity and arm + /// + /// + /// + public static Vector2 AngularVelocityToLinearVelocity(Vector2 a, float w) + { + return new Vector2(-a.Y, a.X) * w; + } + + public static Vector2 Rotate90(Vector2 a) + { + return new Vector2(-a.Y, a.X); + } + + public static Vector2 ProjectPointOnSegment(Vector2 p, Vector2 start, Vector2 end) + { + var u = (end - start).SafeNormalize(Vector2.Zero); + return start + Vector2.Dot(u, p - start) * u; + } + + public static float PointSignedDistanceToLine(Vector2 p, Vector2 start, Vector2 end) + { + Vector2 ab = end - start; + Vector2 ax = p - start; + + float crossProductMagnitude = ab.Cross(ax); + float ABLength = ab.Length(); + + return -crossProductMagnitude / ABLength; + } + + public static float PointSignedDistanceToSegmentGetNearest(Vector2 p, Vector2 start, Vector2 end, out Vector2 pointOnSeg) + { + var u = (end - start).SafeNormalize(Vector2.Zero); + float d = Vector2.Dot(u, p - start); + float sign = u.Cross(p - start) > 0 ? -1 : 1; + if (d <= 0) + { + pointOnSeg = start; + return Vector2.Distance(p, start) * sign; + } + else if (d >= Vector2.Distance(start, end)) + { + pointOnSeg = end; + return Vector2.Distance(p, end) * sign; + } + pointOnSeg = start + d * u; + return (p - (start + d * u)).Length() * sign; + } + + public static float PointDistance2ToSegment(Vector2 p, Vector2 start, Vector2 end) + { + var u = (end - start).SafeNormalize(Vector2.Zero); + float d = Vector2.Dot(u, p - start); + if (d <= 0) + { + return Vector2.DistanceSquared(p, start); + } + else if (d * d >= Vector2.DistanceSquared(start, end)) + { + return Vector2.DistanceSquared(p, end); + } + return (p - (start + d * u)).LengthSquared(); + } + + public static float PointDistance2ToSegmentGetNearest(Vector2 p, Vector2 start, Vector2 end, out Vector2 pointOnSeg) + { + var u = (end - start).SafeNormalize(Vector2.Zero); + float d = Vector2.Dot(u, p - start); + if (d <= 0) + { + pointOnSeg = start; + return Vector2.DistanceSquared(p, start); + } + else if (d * d >= Vector2.DistanceSquared(start, end)) + { + pointOnSeg = end; + return Vector2.DistanceSquared(p, end); + } + pointOnSeg = start + d * u; + return (p - (start + d * u)).LengthSquared(); + } + + public static float PointDistance2ToSegmentWithClip(Vector2 p, Vector2 start, Vector2 end) + { + var u = (end - start).SafeNormalize(Vector2.Zero); + float d = Vector2.Dot(u, p - start); + if (d <= 0) + { + return float.PositiveInfinity; + } + else if (d * d >= Vector2.DistanceSquared(start, end)) + { + return float.PositiveInfinity; + } + return (p - (start + d * u)).LengthSquared(); + } + + public static void SphereConvexPolygonContactInfo(Vector2 cSphere, float radius, List edges, + List corners, Vector2 centerPoly, + List> localPositions) + { + float minDepth = float.PositiveInfinity; + foreach (var edge in edges) + { + float d = PointDistance2ToSegmentWithClip(cSphere, edge._pA, edge._pB); + if (d < minDepth) + { + minDepth = d; + } + } + foreach (var v in corners) + { + float d = Vector2.DistanceSquared(v, cSphere); + if (d < minDepth) + { + minDepth = d; + } + } + + foreach (var edge in edges) + { + float d = PointDistance2ToSegmentWithClip(cSphere, edge._pA, edge._pB); + if (d == minDepth) + { + var p = ProjectPointOnSegment(cSphere, edge._pA, edge._pB); + localPositions.Add(new KeyValuePair( + (p - cSphere).SafeNormalize(Vector2.Zero) * radius, + p - centerPoly + )); + } + } + foreach (var v in corners) + { + float d = Vector2.DistanceSquared(v, cSphere); + if (d == minDepth) + { + localPositions.Add(new KeyValuePair( + (v - cSphere).SafeNormalize(Vector2.Zero) * radius, + v - centerPoly + )); + } + } + } + + public static bool SphereConvexPolygonCollisionInfo(Vector2 cSphere, float radius, List edges, List corners, + out float depth, out Vector2 normal) + { + float maxDepth = float.NegativeInfinity; + normal = Vector2.Zero; + bool anyOutside = false; + foreach (var edge in edges) + { + float d = PointDistance2ToSegmentGetNearest(cSphere, edge._pA, edge._pB, out Vector2 pointOnSeg); + // 如果点在某一个线段的外面，那么就说明点在多边形的外面 + if (d > maxDepth && (edge._pB - edge._pA).Cross(cSphere - edge._pA) <= 0) + { + maxDepth = d; + normal = (cSphere - pointOnSeg).SafeNormalize(Vector2.Zero); + } + + //if (GeometryUtils.Cross(cSphere - edge._pA, edge._pB - edge._pA) <= 0) + //{ + // anyOutside = true; + //} + //if (d > maxDepth) + //{ + // maxDepth = d; + // keyEdge = edge; + //} + } + if (maxDepth < 0) + { + depth = 0; + return false; + } + maxDepth = (float)Math.Sqrt(maxDepth); + depth = radius - maxDepth; + if (maxDepth >= radius) + { + return false; + } + return true; + } + + //public static bool SphereConvexPolygonIsCollide(Vector2 cSphere, float radius, List edges, Vector2 centerPoly) + //{ + // float maxDepth = float.NegativeInfinity; + // foreach (var edge in edges) + // { + // float d = GeometryUtils.PointSignedDistanceToSegment(cSphere, edge._pA, edge._pB); + // if (d > maxDepth) + // { + // maxDepth = d; + // } + // } + // return maxDepth < radius; + //} + + private static (float, float) GetProjectedInterval(List vertices, Vector2 start, Vector2 dir) + { + float v = Vector2.Dot(dir, vertices[0] - start); + float minA = v, maxA = v; + for (int i = 1; i < vertices.Count; i++) + { + float x = Vector2.Dot(dir, vertices[i] - start); + minA = Math.Min(minA, x); + maxA = Math.Max(maxA, x); + } + return (minA, maxA); + } + + public static bool ConvexPolygonPolygonCollisionInfo(List edgesA, List cornersA, + List edgesB, List cornersB, out float depth, out Vector2 normal) + { + depth = float.PositiveInfinity; + normal = Vector2.Zero; + for (int i = 0; i < edgesA.Count; i++) + { + Edge2D edge = edgesA[i]; + Vector2 axis = edge.GetNormal(); + + var (amin, amax) = GetProjectedInterval(cornersA, edge._pA, axis); + var (bmin, bmax) = GetProjectedInterval(cornersB, edge._pA, axis); + + if (Math.Max(amin, bmin) > Math.Min(amax, bmax)) + { + return false; + } + + float d = MinimumSeparatingDistance(amin, amax, bmin, bmax); + if (d < depth) + { + depth = d; + normal = axis; + } + } + for (int i = 0; i < edgesB.Count; i++) + { + Edge2D edge = edgesB[i]; + Vector2 axis = edge.GetNormal(); + + var (amin, amax) = GetProjectedInterval(cornersA, edge._pA, axis); + var (bmin, bmax) = GetProjectedInterval(cornersB, edge._pA, axis); + + if (Math.Max(amin, bmin) > Math.Min(amax, bmax)) + { + return false; + } + + float d = MinimumSeparatingDistance(amin, amax, bmin, bmax); + if (d < depth) + { + depth = d; + normal = axis; + } + } + return true; + } + + public static void ConvexPolygonPolygonContactInfo(List edgesA, List cornersA, + List edgesB, List cornersB, + Vector2 polyCenterA, Vector2 polyCenterB, List> localPositions) + { + float closestDistance = float.PositiveInfinity; + foreach (var curPoint in cornersA) + { + foreach (var curEdge in edgesB) + { + float dist = PointDistance2ToSegment(curPoint, curEdge._pA, curEdge._pB); + if (dist < closestDistance) + { + closestDistance = dist; + localPositions.Clear(); + localPositions.Add(new KeyValuePair( + curPoint - polyCenterA, + ProjectPointOnSegment(curPoint, curEdge._pA, curEdge._pB) + - polyCenterB + )); + } + else if (Math.Abs(dist - closestDistance) < 1e-3) + { + localPositions.Add(new KeyValuePair( + curPoint - polyCenterA, + ProjectPointOnSegment(curPoint, curEdge._pA, curEdge._pB) + - polyCenterB + )); + } + } + } + foreach (var curPoint in cornersB) + { + foreach (var curEdge in edgesA) + { + float dist = PointDistance2ToSegment(curPoint, curEdge._pA, curEdge._pB); + if (dist < closestDistance) + { + closestDistance = dist; + localPositions.Clear(); + localPositions.Add(new KeyValuePair( + ProjectPointOnSegment(curPoint, curEdge._pA, curEdge._pB) - polyCenterA, + curPoint - polyCenterB + )); + } + else if (Math.Abs(dist - closestDistance) < 1e-6) + { + localPositions.Add(new KeyValuePair( + ProjectPointOnSegment(curPoint, curEdge._pA, curEdge._pB) - polyCenterA, + curPoint - polyCenterB + )); + } + } + } + } + + public static bool SphereCapsuleCollisionInfo(Vector2 center, float radius, Vector2 segA, Vector2 segB, + float radius2, out float depth, out Vector2 normal) + { + float d = PointDistance2ToSegmentGetNearest(center, segA, segB, out Vector2 p); + depth = radius + radius2 - (float)Math.Sqrt(d); + normal = Vector2.Zero; + if (d >= (radius + radius2) * (radius + radius2)) + { + return false; + } + normal = (p - center).SafeNormalize(Vector2.Zero); + return true; + } + + public static void SphereCapsuleContactInfo(Vector2 center, float radius, Vector2 segA, Vector2 segB, + float radius2, List> localPositions) + { + var u = (segB - segA).SafeNormalize(Vector2.Zero); + float d_plane = Vector2.Dot(u, center - segA); + float length = (segA - segB).Length(); + Vector2 capsuleCenter = (segA + segB) / 2; + d_plane = MathHelper.Clamp(d_plane, 0, length); + + Vector2 vcenter = segA + u * d_plane; + Vector2 unit = (center - vcenter).SafeNormalize(Vector2.Zero); + localPositions.Add(new KeyValuePair( + -unit * radius, + vcenter + unit * radius2 - capsuleCenter + )); + } + + public static bool CapsuleCapsuleCollisionInfo(Vector2 segA1, Vector2 segB1, + float radius1, Vector2 segA2, Vector2 segB2, + float radius2, out float depth, out Vector2 normal) + { + Vector2 pointA = segA2; + bool flipSign = false; + float d = PointDistance2ToSegmentGetNearest(segA2, segA1, segB1, out Vector2 pointB); + float d2; + if ((d2 = PointDistance2ToSegmentGetNearest(segB2, segA1, segB1, out Vector2 p)) < d) + { + d = d2; + pointA = segB2; + pointB = p; + } + + if ((d2 = PointDistance2ToSegmentGetNearest(segA1, segA2, segB2, out p)) < d) + { + d = d2; + pointA = segA1; + pointB = p; + flipSign = true; + } + + if ((d2 = PointDistance2ToSegmentGetNearest(segB1, segA2, segB2, out p)) < d) + { + d = d2; + pointA = segB1; + pointB = p; + flipSign = true; + } + + + depth = radius1 + radius2 - (float)Math.Sqrt(d); + normal = (pointA - pointB).SafeNormalize(Vector2.UnitX); + // 法线方向必须朝向Sphere碰撞体 + if (flipSign) + { + normal = -normal; + } + if (depth <= 0) + return false; + return true; + } + + public static void CapsuleCapsuleContactInfo(Vector2 segA1, Vector2 segB1, + float radius1, Vector2 segA2, Vector2 segB2, + float radius2, List> localPositions) + { + Vector2 pointA = segA2; + Vector2 capsuleCenter1 = (segA1 + segB1) / 2; + Vector2 capsuleCenter2 = (segA2 + segB2) / 2; + float d = PointDistance2ToSegmentGetNearest(segA2, segA1, segB1, out Vector2 pointB); + Vector2 N = (pointA - pointB).SafeNormalize(Vector2.UnitX); + localPositions.Add(new KeyValuePair( + pointB + N * radius1 - capsuleCenter1, + pointA - N * radius2 - capsuleCenter2 + )); + float d2; + if ((d2 = PointDistance2ToSegmentGetNearest(segB2, segA1, segB1, out Vector2 p)) < d) + { + d = d2; + pointA = segB2; + pointB = p; + localPositions.Clear(); + N = (pointA - pointB).SafeNormalize(Vector2.UnitX); + localPositions.Add(new KeyValuePair( + pointB + N * radius1 - capsuleCenter1, + pointA - N * radius2 - capsuleCenter2 + )); + } + else if (d2 == d) + { + pointA = segB2; + pointB = p; + N = (pointA - pointB).SafeNormalize(Vector2.UnitX); + localPositions.Add(new KeyValuePair( + pointB + N * radius1 - capsuleCenter1, + pointA - N * radius2 - capsuleCenter2 + )); + } + + // 防止四个点全都被取，最多只能保留两个接触点 + if ((d2 = PointDistance2ToSegmentGetNearest(segA1, segA2, segB2, out p)) <= d) + { + d = d2; + pointA = segA1; + pointB = p; + N = (pointA - pointB).SafeNormalize(Vector2.UnitX); + localPositions.Clear(); + localPositions.Add(new KeyValuePair( + pointA - N * radius1 - capsuleCenter1, + pointB + N * radius2 - capsuleCenter2 + )); + } + + if ((d2 = PointDistance2ToSegmentGetNearest(segB1, segA2, segB2, out p)) < d) + { + d = d2; + pointA = segB1; + pointB = p; + N = (pointA - pointB).SafeNormalize(Vector2.UnitX); + localPositions.Clear(); + localPositions.Add(new KeyValuePair( + pointA - N * radius1 - capsuleCenter1, + pointB + N * radius2 - capsuleCenter2 + )); + } + else if (d2 == d) + { + pointA = segB1; + pointB = p; + N = (pointA - pointB).SafeNormalize(Vector2.UnitX); + localPositions.Add(new KeyValuePair( + pointA - N * radius1 - capsuleCenter1, + pointB + N * radius2 - capsuleCenter2 + )); + } + + if (localPositions.Count > 2) + { + // Remove all elements after the first two + localPositions.RemoveRange(2, localPositions.Count - 2); + } + } + + + public static bool CapsuleConvexPolygonCollisionInfo(Vector2 segA1, Vector2 segB1, + float radius1, List corners, List edges, out float depth, out Vector2 normal) + { + depth = float.PositiveInfinity; + normal = Vector2.Zero; + + { + // 中轴线段的法线和中轴线段本身都是关键分离轴 + var edge = new Edge2D(segA1, segB1); + Vector2 axis = edge.GetNormal(); + var (amin, amax) = GetProjectedInterval(new List() { segA1, segB1 }, edge._pA, axis); + var (bmin, bmax) = GetProjectedInterval(corners, edge._pA, axis); + + amin -= radius1; + amax += radius1; + + if (Math.Max(amin, bmin) > Math.Min(amax, bmax)) + { + return false; + } + + float d = MinimumSeparatingDistance(amin, amax, bmin, bmax); + if (d < depth) + { + depth = d; + normal = axis; + } + + axis = (segB1 - segA1).SafeNormalize(Vector2.Zero); + (amin, amax) = GetProjectedInterval(new List() { segA1, segB1 }, edge._pA, axis); + (bmin, bmax) = GetProjectedInterval(corners, edge._pA, axis); + + amin -= radius1; + amax += radius1; + + if (Math.Max(amin, bmin) > Math.Min(amax, bmax)) + { + return false; + } + + d = MinimumSeparatingDistance(amin, amax, bmin, bmax); + if (d < depth) + { + depth = d; + normal = axis; + } + } + + var capsuleCorners = new List() { segA1, segB1 }; + for (int i = 0; i < edges.Count; i++) + { + Edge2D edge = edges[i]; + Vector2 axis = edge.GetNormal(); + + var (amin, amax) = GetProjectedInterval(capsuleCorners, edge._pA, axis); + var (bmin, bmax) = GetProjectedInterval(corners, edge._pA, axis); + + amin -= radius1; + amax += radius1; + + if (Math.Max(amin, bmin) > Math.Min(amax, bmax)) + { + return false; + } + + float d = MinimumSeparatingDistance(amin, amax, bmin, bmax); + if (d < depth) + { + depth = d; + normal = axis; + } + } + return true; + } + + public static void CapsuleConvexPolygonContactInfo(Vector2 segA1, Vector2 segB1, + float radius1, List corners, List edges, Vector2 polyCenter, List> localPositions) + { + float closestDistance = float.PositiveInfinity; + var edgeSeg = new Edge2D(segA1, segB1); + var capsuleVertices = new List() { segA1, segB1 }; + Vector2 capsuleCenter = (segA1 + segB1) / 2; + Vector2 ultimateDir = (polyCenter - capsuleCenter).SafeNormalize(Vector2.Zero); + foreach (var curPoint in corners) + { + float dist = PointDistance2ToSegmentGetNearest(curPoint, edgeSeg._pA, edgeSeg._pB, out Vector2 p); + if (dist < closestDistance) + { + closestDistance = dist; + localPositions.Clear(); + localPositions.Add(new KeyValuePair( + p + (curPoint - p).SafeNormalize(ultimateDir) * radius1 - capsuleCenter, + curPoint - polyCenter + )); + } + else if (Math.Abs(dist - closestDistance) < 1e-6) + { + localPositions.Add(new KeyValuePair( + p + (curPoint - p).SafeNormalize(ultimateDir) * radius1 - capsuleCenter, + curPoint - polyCenter + )); + } + + } + foreach (var capsulePoint in capsuleVertices) + { + foreach (var curEdge in edges) + { + float dist = PointDistance2ToSegmentGetNearest(capsulePoint, curEdge._pA, curEdge._pB, out Vector2 p); + if (dist < closestDistance) + { + closestDistance = dist; + localPositions.Clear(); + localPositions.Add(new KeyValuePair( + capsulePoint + (p - capsulePoint).SafeNormalize(ultimateDir) * radius1 - capsuleCenter, + p - polyCenter + )); + } + else if (Math.Abs(dist - closestDistance) < 1e-6) + { + localPositions.Add(new KeyValuePair( + capsulePoint + (p - capsulePoint).SafeNormalize(ultimateDir) * radius1 - capsuleCenter, + p - polyCenter + )); + } + } + } + + if (localPositions.Count > 2) + { + // Remove all elements after the first two + localPositions.RemoveRange(2, localPositions.Count - 2); + } + } + + + public static float MinimumSeparatingDistance(float amin, float amax, float bmin, float bmax) + { + float d = Math.Min(amax, bmax) - Math.Max(amin, bmin); + if (amin >= bmin && amax <= bmax || amin <= bmin && amax >= bmax) + { + d = Math.Min(amax - bmin, bmax - amin); + } + return d; + } + + public static Vector2 ConvertToPhysicsSpace(Vector2 pos) + { + return new Vector2(pos.X, -pos.Y); + } + + public static void FisherYatesShuffle(this IList list, Random rng) + { + int n = list.Count; + while (n > 1) + { + n--; + int k = rng.Next(n + 1); + T value = list[k]; + list[k] = list[n]; + list[n] = value; + } + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Core/ImpulseEntry.cs b/Sources/Everglow.Function/Physics/PBEngine/Core/ImpulseEntry.cs new file mode 100644 index 000000000..95b0a9b34 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Core/ImpulseEntry.cs @@ -0,0 +1,18 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Everglow.Commons.Physics.PBEngine.Core +{ + public struct ImpulseEntry + { + public RigidBody2D Source; + public RigidBody2D Target; + public Vector2 ImpulseSource; + public Vector2 RelativePositionSource; + public Vector2 ImpulseTarget; + public Vector2 RelativePositionTarget; + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Core/MovementType.cs b/Sources/Everglow.Function/Physics/PBEngine/Core/MovementType.cs new file mode 100644 index 000000000..c4cf91403 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Core/MovementType.cs @@ -0,0 +1,28 @@ +namespace Everglow.Commons.Physics.PBEngine.Core +{ + /// + /// 刚体的移动模式，用于区分模拟方法以及提升性能 + /// + public enum MovementType + { + /// + /// 静态物体，无法移动、转向 + /// + Static, + + /// + /// 动态物体，但是拥有无限质量，动作无法被其他动态物体影响 + /// + Kinematic, + + /// + /// 玩家物体，动态，具有质量但是不可旋转 + /// + Player, + + /// + /// 动态物体，可以任意移动和旋转，可以被任何物体影响 + /// + Dynamic, + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Core/PhysicsObject.cs b/Sources/Everglow.Function/Physics/PBEngine/Core/PhysicsObject.cs new file mode 100644 index 000000000..027d8810b --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Core/PhysicsObject.cs @@ -0,0 +1,199 @@ +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using Everglow.Commons.Utilities; +using Everglow.Commons.Vertex; + +namespace Everglow.Commons.Physics.PBEngine.Core +{ + /// + /// 物理对象的容器，可以容纳Rigidbody，Collider等组件 + /// 此外，它还是物理模拟的主体 + /// + public class PhysicsObject + { + /// + /// 物理对象的几何组件，用于碰撞检测和质心计算等 + /// + public Collider2D Collider + { + get => _collider; + } + + /// + /// 物理对象的刚体物理组件，用于存储和模拟刚体数据 + /// + public RigidBody2D RigidBody + { + get => _rigidBody; + } + + /// + /// 物体的质心位置 + /// + public Vector2 Position + { + get => _rigidBody.CentroidWorldSpace; + set => _rigidBody.CentroidWorldSpace = value; + } + + /// + /// 物体的旋转，绕着质心 + /// + public float Rotation + { + get => _rotation; + set + { + _rotation = value; + _cachedRotationalMatrix = Matrix2x2.CreateRotationMatrix(_rotation); + } + } + + internal Matrix2x2 CachedRotationalMatrix + { + get => _cachedRotationalMatrix; + } + + private Matrix2x2 _cachedRotationalMatrix; + + /// + /// 物体在迭代开始的时候的位置 + /// + public Vector2 OldPosition + { + get => _oldPos; + set => _oldPos = value; + } + + /// + /// 物体在迭代开始的时候的旋转 + /// + public float OldRotation + { + get => _oldRot; + set => _oldRot = value; + } + + /// + /// 物体的GUID + /// + public int GUID + { + get => _guid; + set => _guid = value; + } + + /// + /// 物体的碰撞组名字 + /// + public string Tag + { + get => _tag; + set => _tag = value; + } + + public bool IsActive + { + get => _isActive; + set => _isActive = value; + } + + private Collider2D _collider; + private RigidBody2D _rigidBody; + private Vector2 _oldPos; + private float _rotation; + private float _oldRot; + private int _guid; + private string _tag; + private bool _isActive; + + public PhysicsObject(Collider2D collider, RigidBody2D rigidBody) + { + _collider = collider; + _rigidBody = rigidBody; + _tag = "Default"; + _isActive = true; + + if (_rigidBody == null) + { + _rigidBody = new RigidBody2D(1) + { + MovementType = MovementType.Static, + UseGravity = false, + }; + } + + Rotation = 0; + Position = Vector2.Zero; + + _collider.ParentObject = this; + _rigidBody.ParentObject = this; + } + + public void Initialize() + { + _rigidBody?.Initialize(); + } + + public void RecordOldState() + { + _oldPos = Position; + _oldRot = _rotation; + } + + public List<(Vector2, Color)> GetWireFrameWires() + { + var wires_color = new List<(Vector2, Color)>(); + List wires = _collider.GetWireFrameWires(); + for (int i = 0; i < wires.Count; i++) + { + wires_color.Add((_cachedRotationalMatrix.Multiply(wires[i]) + Position, RigidBody.IsAwake ? Color.White : Color.Gray)); + } + return wires_color; + } + + ///// + ///// Override to add vertex only, no sprite batch. + ///// + ///// + //public virtual List Draw() + //{ + // return new List(); + //} + + public void CleanThisFrame(float deltaTime) + { + _rigidBody?.CleanInformationThisFrame(deltaTime); + } + + public void CleanThisSubstep(float deltaTime) + { + _rigidBody?.CleanInformationSubstep(deltaTime); + Rotation = MathHelper.WrapAngle(Rotation); + } + + public void Update(float deltaTime) + { + _rigidBody?.Update(deltaTime); + } + + public void ApplyGravity(Vector2 gravity) + { + if (!_isActive) + { + return; + } + if (_rigidBody != null) + { + if (_rigidBody.UseGravity) + { + _rigidBody.ApplyForce(gravity * _rigidBody.Mass); + } + } + } + + public Vector2 LocalToWorldPos(Vector2 localPos) + { + return _rigidBody.CentroidWorldSpace + _cachedRotationalMatrix.Multiply(localPos); + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/Core/RigidBody2D.cs b/Sources/Everglow.Function/Physics/PBEngine/Core/RigidBody2D.cs new file mode 100644 index 000000000..727ffa34c --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/Core/RigidBody2D.cs @@ -0,0 +1,734 @@ +using Everglow.Commons.Physics.PBEngine.Collision; +using Everglow.Commons.Utilities; +using Terraria; + +namespace Everglow.Commons.Physics.PBEngine.Core +{ + /// + /// 2D刚体物理组件，用于实际的刚体运动模拟和响应的模块 + /// + public class RigidBody2D + { + /// + /// 指向物理对象的指针 + /// + public PhysicsObject ParentObject + { + get => _bindObject; + set => _bindObject = value; + } + + /// + /// 世界坐标下的质心坐标（指物理世界 + /// + public Vector2 CentroidWorldSpace + { + get => _globalCentroid; + set => _globalCentroid = value; + } + + /// + /// 质量 + /// + public float Mass + { + get => _mass; + set => _mass = value; + } + + /// + /// 质量的倒数 + /// + public float InvMass + { + get => MovementType == MovementType.Dynamic || MovementType == MovementType.Player ? 1 / _mass : 0; + } + + /// + /// 世界坐标下惯性张量的倒数 + /// + public double GlobalInverseInertiaTensor + { + get => MovementType == MovementType.Dynamic ? _globalInverseInertiaTensor : 0; + } + + /// + /// 是否启用重力 + /// + public bool UseGravity + { + get; set; + } + + /// + /// 线速度 + /// + public Vector2 LinearVelocity + { + get => MovementType == MovementType.Static ? Vector2.Zero : _linearVelocity; + set => _linearVelocity = value; + } + + /// + /// 角速度 + /// + public float AngularVelocity + { + get => MovementType == MovementType.Static ? 0 : _angularVelocity; + set => _angularVelocity = value; + } + + /// + /// 移动模式 + /// + public MovementType MovementType + { + get; set; + } + + /// + /// 线速度的阻尼 + /// + public float Drag + { + get => _drag; + set => _drag = value; + } + + /// + /// 角速度的阻尼 + /// + public float AngularDrag + { + get => _angularDrag; + set => _angularDrag = value; + } + + /// + /// 碰撞弹出的系数 + /// + public float Restitution + { + get => _restitution; + set => _restitution = value; + } + + /// + /// 摩擦力系数 + /// + public float Friction + { + get => _friction; + set => _friction = value; + } + + public List TangentRelativeVelocity + { + get => _contactTangentVels; + } + + public List ContactNormals + { + get => _contactNormals; + } + + /// + /// 物体是否是唤醒状态，如果唤醒那么可以进行运动模拟，注意唤醒时会重置运动值 + /// + public bool IsAwake + { + get => _isAwake; + set + { + if (value && !_isAwake) + { + _motion = SleepEPS * 2; + } + else if (!value && _isAwake) + { + _linearVelocity = Vector2.Zero; + _angularVelocity = 0; + } + _isAwake = value; + } + } + + /// + /// 物体是否可以进入沉睡状态，用户能控制的物体应该设为false + /// + public bool CanSleep + { + get => _canSleep; + set => _canSleep = value; + } + + private double _localInverseInertiaTensor; + private double _globalInverseInertiaTensor; + + private Vector2 _globalCentroid; + + private Vector2 _linearVelocity; + private float _angularVelocity; + + private float _torque; + private Vector2 _force; + private float _mass; + private float _drag; + private float _angularDrag; + private float _restitution; + private float _friction; + + private List _contactTangentVels; + private List _contactNormals; + + private List _impulses; + + private bool _isAwake; + private bool _canSleep; + private double _motion; + + private PhysicsObject _bindObject; + + private const double SleepEPS = 1e-2; + + public RigidBody2D(float mass) + { + _mass = mass; + MovementType = MovementType.Dynamic; + UseGravity = true; + _isAwake = true; + _canSleep = true; + _linearVelocity = Vector2.Zero; + _angularVelocity = 0; + _drag = 0.06f; + _angularDrag = 0.06f; + _impulses = new List(); + _restitution = 0.5f; + _friction = 0.5f; + _contactTangentVels = new List(); + _contactNormals = new List(); + + _motion = SleepEPS * 2; + } + + private void CalculateMassCentroidAndMoI() + { + _globalInverseInertiaTensor = _localInverseInertiaTensor = 1 / _bindObject.Collider.InertiaTensor(_mass); + } + + public void Initialize() + { + CalculateMassCentroidAndMoI(); + } + + /// + /// 进行一次运动积分 + /// + /// + public void Update(float deltaTime) + { + if (!_isAwake || MovementType == MovementType.Static) + { + return; + } + + _angularVelocity += (float)(_globalInverseInertiaTensor * deltaTime * _torque); + _linearVelocity += deltaTime * _force / _mass; + + // _oldRot = _rotation; + + // _oldPos = _globalCentroid; + _globalCentroid += deltaTime * _linearVelocity; + _bindObject.Rotation += deltaTime * _angularVelocity; + + Debug.Assert(!float.IsNaN(_linearVelocity.X) && !float.IsNaN(_linearVelocity.Y)); + Debug.Assert(!float.IsNaN(_angularVelocity)); + + StabilizeBody(); + + // CollisionDetection(deltaTime); + // CollisionResponse(deltaTime); + } + + private void StabilizeBody() + { + if (Math.Abs(_linearVelocity.X) < PhysicsSimulation.EPS) + { + _linearVelocity.X = 0; + } + if (Math.Abs(_linearVelocity.Y) < PhysicsSimulation.EPS) + { + _linearVelocity.Y = 0; + } + + if (Math.Abs(_angularVelocity) < PhysicsSimulation.EPS) + { + _angularVelocity = 0; + } + } + + public void ApplyForce(Vector2 force) + { + if (MovementType == MovementType.Static) + { + return; + } + _force += force; + } + + public void CleanInformationSubstep(float deltaTime) + { + _torque = 0; + _force = Vector2.Zero; + _impulses.Clear(); + + // Main.NewText((_linearVelocity.LengthSquared() * _mass * 0.5f + _mass * 9.8f * _globalCentroid.Y).ToString("F1")); + } + + public void CleanInformationThisFrame(float deltaTime) + { + _linearVelocity *= 1 - _drag * _drag; + _angularVelocity *= 1 - _angularDrag * _angularDrag; + _contactTangentVels.Clear(); + _contactNormals.Clear(); + } + + public bool TryRespondTo2Events(List events, float deltaTime) + { + Debug.Assert(events.Count == 2); + Debug.Assert(events[0].Target == events[1].Target); + var target = events[0].Target; + + var ri0 = events[0].LocalOffsetSrc; + var rb0 = events[0].LocalOffsetTarget; + var n0 = events[0].Normal; + var va0 = _linearVelocity + GeometryUtils.AngularVelocityToLinearVelocity(ri0, _angularVelocity); + var vb0 = events[0].Target.RigidBody._linearVelocity + + GeometryUtils.AngularVelocityToLinearVelocity(rb0, events[0].Target.RigidBody._angularVelocity); + float relv0_n = Vector2.Dot(va0 - vb0, n0); + if (relv0_n >= 0) + { + return false; + } + + var ri1 = events[1].LocalOffsetSrc; + var rb1 = events[1].LocalOffsetTarget; + var n1 = events[1].Normal; + var va1 = _linearVelocity + GeometryUtils.AngularVelocityToLinearVelocity(ri1, _angularVelocity); + var vb1 = events[1].Target.RigidBody._linearVelocity + + GeometryUtils.AngularVelocityToLinearVelocity(rb1, events[1].Target.RigidBody._angularVelocity); + float relv1_n = Vector2.Dot(va1 - vb1, n1); + if (relv1_n >= 0) + { + return false; + } + var matrix = new Matrix2x2() + { + [0, 0] = InvMass + events[0].Target.RigidBody.InvMass + + Vector2.Dot( + GeometryUtils.AngularVelocityToLinearVelocity(ri0, (float)(GlobalInverseInertiaTensor + * ri0.Cross(n0))), n0) + + Vector2.Dot( + GeometryUtils.AngularVelocityToLinearVelocity(rb0, (float)(events[0].Target.RigidBody.GlobalInverseInertiaTensor + * rb0.Cross(n0))), n0), + [0, 1] = InvMass * Vector2.Dot(n0, n1) + Vector2.Dot( + GeometryUtils.AngularVelocityToLinearVelocity(ri0, (float)(GlobalInverseInertiaTensor + * ri1.Cross(n1))), n0), + + [1, 0] = InvMass * Vector2.Dot(n0, n1) + Vector2.Dot( + GeometryUtils.AngularVelocityToLinearVelocity(ri1, (float)(GlobalInverseInertiaTensor + * ri0.Cross(n0))), n1), + [1, 1] = InvMass + events[1].Target.RigidBody.InvMass + + Vector2.Dot( + GeometryUtils.AngularVelocityToLinearVelocity(ri1, (float)(GlobalInverseInertiaTensor + * ri1.Cross(n1))), n1) + + Vector2.Dot( + GeometryUtils.AngularVelocityToLinearVelocity(rb1, (float)(events[1].Target.RigidBody.GlobalInverseInertiaTensor + * rb1.Cross(n1))), n1), + }; + float C = 0.4f; + var J = matrix.Inverse().Multiply(new Vector2(-(1 + C) * relv0_n, -(1 + C) * relv1_n)); + if (!J.HasNaNs()) + { + AddImpulse(J.X * events[0].Normal, ri0, target.RigidBody, -J.X * events[0].Normal, rb0); + AddImpulse(J.Y * events[1].Normal, ri1, target.RigidBody, -J.Y * events[1].Normal, rb1); + } + return true; + } + + public void ApplyAngularVelocity(float w) + { + _angularVelocity += w; + } + + private void SolveContactImpulse(CollisionEvent2D e, int count, float deltaTime) + { + var ri = e.LocalOffsetSrc; + var rb = e.LocalOffsetTarget; + + var v2 = GeometryUtils.AngularVelocityToLinearVelocity(ri, _angularVelocity); + var va = _linearVelocity + v2; + var vb = e.Target.RigidBody._linearVelocity + + GeometryUtils.AngularVelocityToLinearVelocity(rb, e.Target.RigidBody._angularVelocity); + var bias = 0.1f * Math.Max(e.Depth - 0.02f, 0) / deltaTime; + + float va_n = Vector2.Dot(va - vb, e.Normal); + if (MovementType == MovementType.Player) + { + _contactNormals.Add(e.Normal); + } + if (e.Target.RigidBody.MovementType == MovementType.Player) + { + e.Target.RigidBody._contactNormals.Add(-e.Normal); + } + if (va_n >= 0) + { + e.NormalVelOld = 0; + return; + } + float stiffness = Math.Max(0, (_restitution + e.Target.RigidBody.Restitution) / 2); + float vnew_n = stiffness * Math.Max(-va_n - 20 * deltaTime, 0); + + // (a × b) × c = (c • a)b - (c • b)a + double rAdotN = Vector2.Dot(GeometryUtils.Rotate90(ri), e.Normal); + double rBdotN = Vector2.Dot(GeometryUtils.Rotate90(rb), e.Normal); + double R1 = rAdotN * rAdotN * GlobalInverseInertiaTensor; // Vector2.Dot(Utils.AnuglarVelocityToLinearVelocity(ri, (float)(GlobalInverseInertiaTensor + + // * Utils.Cross(ri, e.Normal))), e.Normal); + double R2 = rBdotN * rBdotN * e.Target.RigidBody.GlobalInverseInertiaTensor; // Vector2.Dot(Utils.AnuglarVelocityToLinearVelocity(rb, (float)(e.Target.RigidBody.GlobalInverseInertiaTensor + + // * Utils.Cross(rb, e.Normal))), e.Normal); + double J_n = (vnew_n - va_n) / (InvMass + e.Target.RigidBody.InvMass + R1 + R2); + Vector2 J = (float)J_n * e.Normal / count; // + (float)J_t * va_t_unit; + e.NormalVelOld = (float)J_n; + + // var offset = e.Position - (_globalCentroid + ri); + AddImpulse(J, ri, e.Target.RigidBody, -J, rb); + Debug.Assert(!float.IsNaN(J.X) && !float.IsNaN(J.Y)); + } + + private void SolveFrictionImpulse(CollisionEvent2D e, int count, float deltaTime) + { + var ri = e.LocalOffsetSrc; + var rb = e.LocalOffsetTarget; + + var v2 = GeometryUtils.AngularVelocityToLinearVelocity(ri, _angularVelocity); + var va = _linearVelocity + v2; + var vb = e.Target.RigidBody._linearVelocity + + GeometryUtils.AngularVelocityToLinearVelocity(rb, e.Target.RigidBody._angularVelocity); + + if (e.NormalVelOld == 0) + { + return; + } + + float vel_n = Vector2.Dot(va - vb, e.Normal); + Vector2 vt = va - vb - vel_n * e.Normal; + + float vel_t = vt.Length(); + float friction = Math.Max(0, (_friction + e.Target.RigidBody.Friction) / 2); + + vt = vt.SafeNormalize(Vector2.Zero); + if (vt.Length() == 0) + { + return; + } + + _contactTangentVels.Add(-Vector2.Dot(vt, vb) * vt); + e.Target.RigidBody._contactTangentVels.Add(-Vector2.Dot(vt, va) * vt); + if (friction == 0) + { + return; + } + + double rAdotN = Vector2.Dot(GeometryUtils.Rotate90(ri), vt); + double rBdotN = Vector2.Dot(GeometryUtils.Rotate90(rb), vt); + double R1 = rAdotN * rAdotN * GlobalInverseInertiaTensor; // Vector2.Dot(Utils.AnuglarVelocityToLinearVelocity(ri, (float)(GlobalInverseInertiaTensor + + // * Utils.Cross(ri, e.Normal))), e.Normal); + double R2 = rBdotN * rBdotN * e.Target.RigidBody.GlobalInverseInertiaTensor; // Vector2.Dot(Utils.AnuglarVelocityToLinearVelocity(rb, (float)(e.Target.RigidBody.GlobalInverseInertiaTensor + + // * Utils.Cross(rb, e.Normal))), e.Normal); + double effectiveMass = InvMass + e.Target.RigidBody.InvMass + R1 + R2; + + // (a × b) × c = (c • a)b - (c • b)a + double J_n = -Math.Min(friction * e.NormalVelOld, vel_t / effectiveMass); + Vector2 J = (float)J_n * vt / count; // + (float)J_t * va_t_unit; + + AddImpulse(J, ri, e.Target.RigidBody, -J, rb); + Debug.Assert(!float.IsNaN(J.X) && !float.IsNaN(J.Y)); + } + + /// + /// 对接触事件进行响应 + /// + /// + /// + public void RespondToEvents(List events, float deltaTime) + { + foreach (var e in events) + { + SolveContactImpulse(e, events.Count, deltaTime); + } + ApplyImpulses(); + foreach (var e in events) + { + SolveFrictionImpulse(e, events.Count, deltaTime); + MatchAwakeState(e.Target.RigidBody, deltaTime); + } + ApplyImpulses(); + } + + public void Integrate(float deltaTime) + { + if (MovementType == MovementType.Static) + { + return; + } + _bindObject.Rotation += deltaTime * _angularVelocity; + _globalCentroid += deltaTime * _linearVelocity; + _bindObject.Position = _globalCentroid; + } + + // public void ResolveImpluse(float deltaTime) + // { + // if (_impulses.Count == 0) + // { + // return; + // } + // //_bindObject.Rotation = _bindObject.OldRotation; + // //_globalCentroid = _bindObject.OldPosition; + // _impulses.Sort((a, b) => + // { + // return a.Time.CompareTo(b.Time); + // }); + // float lastTime = 0; + // int sameTimeCount = 0; + // for (int i = 0; i < _impulses.Count; i++) + // { + // sameTimeCount++; + // if (i == _impulses.Count - 1 || _impulses[i].Time != _impulses[i + 1].Time) + // { + // float dt = (_impulses[i].Time - lastTime); + + // Vector2 linearVelocityChange = Vector2.Zero; + // float angularVelocityChange = 0; + // Vector2 normalAvg = Vector2.Zero; + + // //linearVelocityChange /= sameTimeCount; + // //angularVelocityChange /= sameTimeCount; + // //normalAvg /= sameTimeCount; + // //normalAvg = normalAvg.SafeNormalize(Vector2.Zero); + + // var resImpluses = ResolveConstrains(); + // for (int j = i - sameTimeCount + 1; j <= i; j++) + // { + // linearVelocityChange += 1.0f / _mass * resImpluses[j] * _impulses[j].Normal; + // angularVelocityChange += (float)(Utils.Cross(_impulses[j].RelativePosition, + // resImpluses[j] *_impulses[j].Normal * (float)_globalInverseInertiaTensor)); + // } + + // //var penetration = Math.Max(0, -Vector2.Dot(_linearVelocity * deltaTime, normalAvg)); + // //_linearVelocity += normalAvg * penetration * 0.7f; + + // deltaTime -= dt; + // lastTime = _impulses[i].Time; + // sameTimeCount = 0; + // } + // } + // _impulses.Clear(); + // } + + // private List ResolveConstrains() + // { + // List impluseArray = new List(); + // List impluseArrayTemp = new List(); + // for (int k = 0; k < _impulses.Count; k++) + // { + // impluseArray.Add(_impulses[k].Impluse.Length()); + // impluseArrayTemp.Add(0f); + // } + // for (int iter = 0; iter < 8; iter++) + // { + // int sameSourceCount = 0; + // for (int k = 0; k < _impulses.Count; k++) + // { + // var ri0 = _impulses[k].CollisionEvent.LocalOffsetSrc; + // var rb0 = _impulses[k].CollisionEvent.LocalOffsetTarget; + // var n0 = _impulses[k].CollisionEvent.Normal; + // var va0 = _impulses[k].CollisionEvent.Source.RigidBody.LinearVelocity + // + Utils.AnuglarVelocityToLinearVelocity(ri0, _impulses[k].CollisionEvent.Source.RigidBody.AngularVelocity); + // var vb0 = _impulses[k].CollisionEvent.Target.RigidBody.LinearVelocity + // + Utils.AnuglarVelocityToLinearVelocity(rb0, _impulses[k].CollisionEvent.Target.RigidBody.AngularVelocity); + // float relv0_n = Vector2.Dot(va0 - vb0, n0); + // float relv0_plus = -(1 + 0.4f) * Vector2.Dot(va0 - vb0, n0); + + // float Kj = _impulses[k].CollisionEvent.Source.RigidBody.InvMass + _impulses[k].CollisionEvent.Target.RigidBody.InvMass + // + Vector2.Dot(Utils.AnuglarVelocityToLinearVelocity(ri0, (float)(_impulses[k].CollisionEvent.Source.RigidBody.GlobalInverseInertiaTensor + // * Utils.Cross(ri0, n0))), n0) + // + Vector2.Dot(Utils.AnuglarVelocityToLinearVelocity(rb0, (float)(_impulses[k].CollisionEvent.Target.RigidBody.GlobalInverseInertiaTensor + // * Utils.Cross(rb0, n0))), n0); + + // float KLU = 0f; + // for (int l = 0; l < _impulses.Count; l++) + // { + // if (l == k) + // { + // continue; + // } + // var ri1 = _impulses[l].CollisionEvent.LocalOffsetSrc; + // var rb1 = _impulses[l].CollisionEvent.LocalOffsetTarget; + // var n1 = _impulses[l].CollisionEvent.Normal; + + // KLU += (_impulses[k].CollisionEvent.Source.RigidBody.InvMass + // * Vector2.Dot(n0, n1) + Vector2.Dot(Utils.AnuglarVelocityToLinearVelocity(ri0, + // (float)(_impulses[k].CollisionEvent.Source.RigidBody.GlobalInverseInertiaTensor + // * Utils.Cross(ri1, n1))), n0)) * impluseArray[l]; + // } + // impluseArrayTemp[k] = 1 / Kj * (relv0_plus - KLU); + // } + // sameSourceCount = 0; + // // Deep copy + // for (int k = 0; k < impluseArray.Count; k++) + // { + // impluseArray[k] = impluseArrayTemp[k]; + // } + // } + + // return impluseArray; + // } + public void AddForce(Vector2 force, Vector2 relPos) + { + _force += force; + _torque += relPos.Cross(force); + } + + /// + /// 把冲量数据累加到这个刚体上 + /// + /// + /// + /// + /// + /// + public void AddImpulse(Vector2 J, Vector2 relativePos, RigidBody2D other, Vector2 J2, Vector2 relativePos2) + { + var entry = new ImpulseEntry() + { + Source = this, + Target = other, + ImpulseSource = J, + RelativePositionSource = relativePos, + ImpulseTarget = J2, + RelativePositionTarget = relativePos2, + }; + _impulses.Add(entry); + } + + /// + /// 对累加的冲量进行应用，更新各种速度 + /// + public void ApplyImpulses() + { + if (_impulses.Count > 0) + { + foreach (var imp in _impulses) + { + if (imp.Source.MovementType == MovementType.Dynamic || imp.Source.MovementType == MovementType.Player) + { + _linearVelocity += 1.0f / _mass * imp.ImpulseSource; + if (imp.Source.MovementType != MovementType.Player) + { + _angularVelocity += imp.RelativePositionSource.Cross(imp.ImpulseSource * (float)_globalInverseInertiaTensor); + } + } + if (imp.Target.MovementType == MovementType.Dynamic || imp.Target.MovementType == MovementType.Player) + { + imp.Target._linearVelocity += imp.Target.InvMass * imp.ImpulseTarget; + if (imp.Target.MovementType != MovementType.Player) + { + imp.Target._angularVelocity += GeometryUtils.Cross(imp.RelativePositionTarget, imp.ImpulseTarget * (float)imp.Target.GlobalInverseInertiaTensor); + } + } + } + } + + Debug.Assert(!float.IsNaN(_linearVelocity.X) && !float.IsNaN(_linearVelocity.Y)); + Debug.Assert(!float.IsNaN(_angularVelocity)); + StabilizeBody(); + _impulses.Clear(); + } + + /// + /// 立即施加一个冲量 + /// + /// + /// + public void AddImpulseImmediate(Vector2 J, Vector2 relativePos, RigidBody2D other, Vector2 J2, Vector2 relativePos2) + { + if (MovementType != MovementType.Static && MovementType != MovementType.Kinematic) + { + _linearVelocity += 1.0f / _mass * J; + Debug.Assert(!float.IsNaN(_linearVelocity.X) && !float.IsNaN(_linearVelocity.Y)); + _angularVelocity += relativePos.Cross( + J * (float)GlobalInverseInertiaTensor); + Debug.Assert(!float.IsNaN(_angularVelocity)); + } + + if (other.MovementType != MovementType.Static && MovementType != MovementType.Kinematic) + { + other._linearVelocity += other.InvMass * J2; + other._angularVelocity += relativePos2.Cross( + J2 * (float)other.GlobalInverseInertiaTensor); + } + } + + public void MoveBody(Vector2 dir, float deltaTime) + { + _globalCentroid += dir; + + // _linearVelocity = (_globalCentroid - ParentObject.OldPosition) / deltaTime; + } + + public void MatchAwakeState(RigidBody2D other, float deltaTime) + { + if (other.MovementType == MovementType.Static) + { + return; + } + if (!_isAwake && !other._isAwake) + { + return; + } + if (_isAwake && !other._isAwake) + { + other.IsAwake = true; + } + else if (!_isAwake && other._isAwake) + { + IsAwake = true; + } + } + + public void CheckAwake(float deltaTime) + { + if (MovementType == MovementType.Static || MovementType == MovementType.Kinematic) + { + return; + } + double bias = Math.Pow(0.6, deltaTime); + _motion = bias * _motion + (1 - bias) * (_linearVelocity.LengthSquared() + + 3 * _angularVelocity * _angularVelocity); + + if (_motion > 16 * SleepEPS) + { + _motion = 16 * SleepEPS; + } + + if (_motion < SleepEPS) + { + IsAwake = false; + } + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/GameInteraction/TileCollider.cs b/Sources/Everglow.Function/Physics/PBEngine/GameInteraction/TileCollider.cs new file mode 100644 index 000000000..75fd41e9d --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/GameInteraction/TileCollider.cs @@ -0,0 +1,48 @@ +using Everglow.Commons.Physics.PBEngine.Collision; +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; + +namespace Everglow.Commons.Physics.PBEngine.GameInteraction +{ + /// + /// 与TR世界地形碰撞的碰撞体 + /// + public class TileCollider : Collider2D + { + public override AABB GetAABB(float deltaTime) + { + return new AABB() + { + MinPoint = new Vector2(float.NegativeInfinity, float.NegativeInfinity), + MaxPoint = new Vector2(float.PositiveInfinity, 0), + }; + } + + public override void GetContactInfo(in CollisionInfo info, float deltaTime, out List collisionEvents) + { + // Leave empty since static object will not serve as collision source + collisionEvents = new List(); + } + + // public override List GetContactInfo(CollisionEvent2D e, float deltaTime) + // { + // return new List(); + // // Leave empty since static object will not serve as collision source + // } + public override List GetWireFrameWires() + { + return new List(); + } + + public override double InertiaTensor(float mass) + { + return 1; + } + + public override bool TestCollisionCondition(Collider2D other, float deltaTime, out CollisionInfo info) + { + // Leave empty since static object will not serve as collision source + info = default(CollisionInfo); + return false; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/GameInteraction/TileCollisionUtils.cs b/Sources/Everglow.Function/Physics/PBEngine/GameInteraction/TileCollisionUtils.cs new file mode 100644 index 000000000..2d2a7bab7 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/GameInteraction/TileCollisionUtils.cs @@ -0,0 +1,411 @@ +using Everglow.Commons.Physics.PBEngine.Collision; +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using Everglow.Commons.Physics.PBEngine.Collision.Shapes; +using Everglow.Commons.Physics.PBEngine.Core; +using Terraria; + +namespace Everglow.Commons.Physics.PBEngine.GameInteraction +{ + /// + /// 与TR地形进行碰撞检测的函数 + /// + public class TileCollisionUtils + { + public static List GenerateHalfBrick(int x, int y) + { + var slope = Main.tile[x, y].Slope; + if (slope == Terraria.ID.SlopeType.Solid) + { + if (Main.tile[x, y].IsHalfBlock) + { + return new List() + { + new Vector2(x * 16, -y * 16 - 16), + new Vector2(x * 16 + 16, -y * 16 - 16), + new Vector2(x * 16 + 16, -y * 16 - 8 ), + new Vector2(x * 16, -y * 16 - 8), + }; + } + else + { + return new List() + { + new Vector2(x * 16, -y * 16 - 16), + new Vector2(x * 16 + 16, -y * 16 - 16), + new Vector2(x * 16 + 16, -y * 16 ), + new Vector2(x * 16, -y * 16), + }; + } + } + else if (slope == Terraria.ID.SlopeType.SlopeDownLeft) + { + return new List() + { + new Vector2(x * 16, -y * 16 - 16), + new Vector2(x * 16 + 16, -y * 16 - 16), + new Vector2(x * 16, -y * 16), + }; + } + else if (slope == Terraria.ID.SlopeType.SlopeDownRight) + { + return new List() + { + new Vector2(x * 16, -y * 16 - 16), + new Vector2(x * 16 + 16, -y * 16 - 16), + new Vector2(x * 16 + 16, -y * 16 ), + }; + } + else if (slope == Terraria.ID.SlopeType.SlopeUpLeft) + { + return new List() + { + new Vector2(x * 16, -y * 16 - 16), + new Vector2(x * 16 + 16, -y * 16 ), + new Vector2(x * 16, -y * 16), + }; + } + else if (slope == Terraria.ID.SlopeType.SlopeUpRight) + { + return new List() + { + new Vector2(x * 16 + 16, -y * 16 - 16), + new Vector2(x * 16 + 16, -y * 16 ), + new Vector2(x * 16, -y * 16), + }; + } + return new List() + { + new Vector2(x * 16, -y * 16 - 16), + new Vector2(x * 16 + 16, -y * 16 - 16), + new Vector2(x * 16 + 16, -y * 16 ), + new Vector2(x * 16, -y * 16), + }; + } + + public static bool GetPolygonTileCollisionInfo(AABB boundingBox, Vector2 polyCenter, List edges, List corners, + out float depth1, out Vector2 normal1) + { + bool collided = false; + float maxDepth = 0; + Vector2 maxNormal = Vector2.Zero; + int minX = (int)Math.Floor(boundingBox.MinPoint.X / 16); + int maxX = (int)Math.Ceiling(boundingBox.MaxPoint.X / 16); + int minY = (int)Math.Floor((-boundingBox.MaxPoint.Y) / 16); + int maxY = (int)Math.Ceiling((-boundingBox.MinPoint.Y) / 16); + + for (int x = Math.Max(minX, 0); x <= Math.Min(maxX, Main.maxTilesX - 1); x++) + { + for (int y = Math.Max(minY, 0); y <= Math.Min(maxY, Main.maxTilesY - 1); y++) + { + if (Main.tile[x, y] != null && Main.tile[x, y].HasTile && Main.tileSolid[Main.tile[x, y].TileType]) + { + var localPoints = GenerateHalfBrick(x, y); + List edgesTile = new List(); + for (int i = 0; i < localPoints.Count; i++) + { + edgesTile.Add(new Edge2D(localPoints[i], localPoints[(i + 1) % localPoints.Count])); + } + var centerTile = new Vector2(x * 16 + 8, -y * 16 - 8); + float depth; + Vector2 normal; + if (GeometryUtils.ConvexPolygonPolygonCollisionInfo(edges, corners, edgesTile, localPoints, + out depth, out normal)) + { + depth1 = 0; + normal1 = Vector2.Zero; + return true; + + if (depth > maxDepth) + { + maxDepth = depth; + maxNormal = Vector2.Dot(maxNormal, polyCenter - centerTile) < 0 ? -normal : normal; + } + } + } + } + } + depth1 = 0; + normal1 = maxNormal; + return collided; + } + + public static void GetPolygonTileContactInfo(AABB boundingBox, Vector2 polyCenter, List edges, + List corners, + PhysicsObject A, PhysicsObject B, + float deltaTime, + List outputList) + { + int minX = (int)Math.Floor(boundingBox.MinPoint.X / 16); + int maxX = (int)Math.Ceiling(boundingBox.MaxPoint.X / 16); + int minY = (int)Math.Floor((-boundingBox.MaxPoint.Y) / 16); + int maxY = (int)Math.Ceiling((-boundingBox.MinPoint.Y) / 16); + + for (int x = Math.Max(minX, 0); x <= Math.Min(maxX, Main.maxTilesX - 1); x++) + { + for (int y = Math.Max(minY, 0); y <= Math.Min(maxY, Main.maxTilesY - 1); y++) + { + if (Main.tile[x, y] != null && Main.tile[x, y].HasTile && Main.tileSolid[Main.tile[x, y].TileType]) + { + var localPoints = GenerateHalfBrick(x, y); + List edgesTile = new List(); + for (int i = 0; i < localPoints.Count; i++) + { + edgesTile.Add(new Edge2D(localPoints[i], localPoints[(i + 1) % localPoints.Count])); + } + var centerTile = new Vector2(x * 16 + 8, -y * 16 - 8); + + var boxCollider = A.Collider as BoxCollider; + float depth; + Vector2 normal; + if (GeometryUtils.ConvexPolygonPolygonCollisionInfo( + edges, + corners, edgesTile, localPoints, + out depth, out normal)) + { + var e = new CollisionEvent2D() + { + Time = 0, + Source = A, + Target = B, + LocalOffsetSrc = Vector2.Zero, + LocalOffsetTarget = Vector2.Zero, + Normal = Vector2.Dot(normal, polyCenter - centerTile) < 0 ? -normal : normal, + Position = Vector2.Zero, + Depth = depth, + }; + float weightA = e.Source.RigidBody.InvMass / (e.Source.RigidBody.InvMass + e.Target.RigidBody.InvMass); + float weightB = e.Target.RigidBody.InvMass / (e.Source.RigidBody.InvMass + e.Target.RigidBody.InvMass); + e.Source.RigidBody.MoveBody(e.Normal * weightA * e.Depth, deltaTime); + e.Target.RigidBody.MoveBody(-e.Normal * weightB * e.Depth, deltaTime); + + List> contacts = new List>(); + GeometryUtils.ConvexPolygonPolygonContactInfo( + boxCollider.GetEdges(deltaTime), + boxCollider.GetCornerPoints(deltaTime), edgesTile, localPoints.ToList(), polyCenter, + centerTile, contacts); + foreach (var c in contacts) + { + outputList.Add(new CollisionEvent2D(e) + { + LocalOffsetSrc = c.Key, + LocalOffsetTarget = c.Value, + }); + } + } + } + } + } + } + + public static bool GetSphereTileCollisionInfo(Vector2 center, float radius, PhysicsObject A, PhysicsObject B, + out float depth1, out Vector2 normal1) + { + bool collided = false; + float maxDepth = 0; + Vector2 maxNormal = Vector2.Zero; + int minX = (int)Math.Floor((center.X - radius) / 16); + int maxX = (int)Math.Ceiling((center.X + radius) / 16); + int minY = (int)Math.Floor((-center.Y - radius) / 16); + int maxY = (int)Math.Ceiling((-center.Y + radius) / 16); + + for (int x = Math.Max(minX, 0); x <= Math.Min(maxX, Main.maxTilesX - 1); x++) + { + for (int y = Math.Max(minY, 0); y <= Math.Min(maxY, Main.maxTilesY - 1); y++) + { + if (Main.tile[x, y] != null && Main.tile[x, y].HasTile && Main.tileSolid[Main.tile[x, y].TileType]) + { + var localPoints = GenerateHalfBrick(x, y); + List edges = new List(); + for (int i = 0; i < localPoints.Count; i++) + { + edges.Add(new Edge2D(localPoints[i], localPoints[(i + 1) % localPoints.Count])); + } + var centerTile = new Vector2(x * 16 + 8, -y * 16 - 8); + float depth; + Vector2 normal; + if (GeometryUtils.SphereConvexPolygonCollisionInfo(center, radius, edges, localPoints.ToList(), out depth, out normal)) + { + collided = true; + if (depth > maxDepth) + { + maxDepth = depth; + maxNormal = Vector2.Dot(maxNormal, center - centerTile) < 0 ? -normal : normal; + } + } + } + } + } + depth1 = 0; + normal1 = maxNormal; + return collided; + } + + public static void GetSphereTileContactInfo(Vector2 center, float radius, PhysicsObject A, PhysicsObject B, + float deltaTime, + List outputList) + { + int minX = (int)Math.Floor((center.X - radius) / 16); + int maxX = (int)Math.Ceiling((center.X + radius) / 16); + int minY = (int)Math.Floor((-center.Y - radius) / 16); + int maxY = (int)Math.Ceiling((-center.Y + radius) / 16); + + for (int x = Math.Max(minX, 0); x <= Math.Min(maxX, Main.maxTilesX - 1); x++) + { + for (int y = Math.Max(minY, 0); y <= Math.Min(maxY, Main.maxTilesY - 1); y++) + { + if (Main.tile[x, y] != null && Main.tile[x, y].HasTile && Main.tileSolid[Main.tile[x, y].TileType]) + { + var localPoints = GenerateHalfBrick(x, y); + List edges = new List(); + for (int i = 0; i < localPoints.Count; i++) + { + edges.Add(new Edge2D(localPoints[i], localPoints[(i + 1) % localPoints.Count])); + } + var centerTile = new Vector2(x * 16 + 8, -y * 16 - 8); + float depth; + Vector2 normal; + if (GeometryUtils.SphereConvexPolygonCollisionInfo(A.RigidBody.CentroidWorldSpace, radius, edges, localPoints.ToList(), out depth, out normal)) + { + var e = new CollisionEvent2D() + { + Time = 0, + Source = A, + Target = B, + LocalOffsetSrc = Vector2.Zero, + LocalOffsetTarget = Vector2.Zero, + Normal = Vector2.Dot(normal, center - centerTile) < 0 ? -normal : normal, + Position = Vector2.Zero, + Depth = depth, + }; + float weightA = e.Source.RigidBody.InvMass / (e.Source.RigidBody.InvMass + e.Target.RigidBody.InvMass); + float weightB = e.Target.RigidBody.InvMass / (e.Source.RigidBody.InvMass + e.Target.RigidBody.InvMass); + e.Source.RigidBody.MoveBody(e.Normal * weightA * e.Depth, deltaTime); + e.Target.RigidBody.MoveBody(-e.Normal * weightB * e.Depth, deltaTime); + + List> contacts = new List>(); + GeometryUtils.SphereConvexPolygonContactInfo(e.Source.RigidBody.CentroidWorldSpace, radius, edges, localPoints.ToList(), centerTile, contacts); + foreach (var c in contacts) + { + outputList.Add(new CollisionEvent2D(e) + { + LocalOffsetSrc = c.Key, + LocalOffsetTarget = c.Value, + }); + } + } + } + } + } + } + + public static bool GetCapsuleTileCollisionInfo(AABB boundingBox, Vector2 segA, Vector2 segB, float radius, + out float depth1, out Vector2 normal1) + { + bool collided = false; + float maxDepth = 0; + Vector2 maxNormal = Vector2.Zero; + int minX = (int)Math.Floor(boundingBox.MinPoint.X / 16); + int maxX = (int)Math.Ceiling(boundingBox.MaxPoint.X / 16); + int minY = (int)Math.Floor((-boundingBox.MaxPoint.Y) / 16); + int maxY = (int)Math.Ceiling((-boundingBox.MinPoint.Y) / 16); + + for (int x = Math.Max(minX, 0); x <= Math.Min(maxX, Main.maxTilesX - 1); x++) + { + for (int y = Math.Max(minY, 0); y <= Math.Min(maxY, Main.maxTilesY - 1); y++) + { + if (Main.tile[x, y] != null && Main.tile[x, y].HasTile && Main.tileSolid[Main.tile[x, y].TileType]) + { + var localPoints = GenerateHalfBrick(x, y); + List edges = new List(); + for (int i = 0; i < localPoints.Count; i++) + { + edges.Add(new Edge2D(localPoints[i], localPoints[(i + 1) % localPoints.Count])); + } + var centerTile = new Vector2(x * 16 + 8, -y * 16 - 8); + float depth; + Vector2 normal; + if (GeometryUtils.CapsuleConvexPolygonCollisionInfo(segA, segB, radius, localPoints.ToList(), edges, + out depth, out normal)) + { + collided = true; + + if (depth > maxDepth) + { + maxDepth = depth; + maxNormal = Vector2.Dot(maxNormal, boundingBox.Center - centerTile) < 0 ? -normal : normal; + } + } + } + } + } + depth1 = 0; + normal1 = maxNormal; + return collided; + } + + public static void GetCapsuleTileContactInfo(AABB boundingBox, Vector2 segA, Vector2 segB, float radius, + PhysicsObject A, PhysicsObject B, + float deltaTime, + List outputList) + { + int minX = (int)Math.Floor(boundingBox.MinPoint.X / 16); + int maxX = (int)Math.Ceiling(boundingBox.MaxPoint.X / 16); + int minY = (int)Math.Floor((-boundingBox.MaxPoint.Y) / 16); + int maxY = (int)Math.Ceiling((-boundingBox.MinPoint.Y) / 16); + + var capsule = A.Collider as CapsuleCollider; + + for (int x = Math.Max(minX, 0); x <= Math.Min(maxX, Main.maxTilesX - 1); x++) + { + for (int y = Math.Max(minY, 0); y <= Math.Min(maxY, Main.maxTilesY - 1); y++) + { + if (Main.tile[x, y] != null && Main.tile[x, y].HasTile && Main.tileSolid[Main.tile[x, y].TileType]) + { + var localPoints = GenerateHalfBrick(x, y); + List edges = new List(); + for (int i = 0; i < localPoints.Count; i++) + { + edges.Add(new Edge2D(localPoints[i], localPoints[(i + 1) % localPoints.Count])); + } + var centerTile = new Vector2(x * 16 + 8, -y * 16 - 8); + float depth; + Vector2 normal; + + if (GeometryUtils.CapsuleConvexPolygonCollisionInfo(segA, segB, radius, localPoints.ToList(), edges, + out depth, out normal)) + { + var e = new CollisionEvent2D() + { + Time = 0, + Source = A, + Target = B, + LocalOffsetSrc = Vector2.Zero, + LocalOffsetTarget = Vector2.Zero, + Normal = Vector2.Dot(normal, boundingBox.Center - centerTile) < 0 ? -normal : normal, + Position = Vector2.Zero, + Depth = depth, + }; + float weightA = e.Source.RigidBody.InvMass / (e.Source.RigidBody.InvMass + e.Target.RigidBody.InvMass); + float weightB = 1 - weightA; + e.Source.RigidBody.MoveBody(e.Normal * weightA * e.Depth, deltaTime); + e.Target.RigidBody.MoveBody(-e.Normal * weightB * e.Depth, deltaTime); + + List> contacts = new List>(); + capsule.GetSegment(deltaTime, out Vector2 segAt, out Vector2 segBt); + GeometryUtils.CapsuleConvexPolygonContactInfo(segAt, segBt, radius, localPoints.ToList(), edges, + centerTile, contacts); + foreach (var c in contacts) + { + outputList.Add(new CollisionEvent2D(e) + { + LocalOffsetSrc = c.Key, + LocalOffsetTarget = c.Value, + }); + } + } + } + } + } + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/PhysicsSimulation.cs b/Sources/Everglow.Function/Physics/PBEngine/PhysicsSimulation.cs new file mode 100644 index 000000000..a64eb0f2b --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/PhysicsSimulation.cs @@ -0,0 +1,547 @@ +using Everglow.Commons.DataStructures; +using Everglow.Commons.Physics.PBEngine.Collision; +using Everglow.Commons.Physics.PBEngine.Collision.BroadPhase; +using Everglow.Commons.Physics.PBEngine.Collision.BroadPhase.Structure; +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using Everglow.Commons.Physics.PBEngine.Constraints; +using Everglow.Commons.Physics.PBEngine.Core; +using Everglow.Commons.Vertex; +using Terraria; + +namespace Everglow.Commons.Physics.PBEngine +{ + /// + /// 用于物理模拟的世界，可以设置各种世界属性，添加和管理物理对象 + /// 其中最重要的模拟部分也是由这个类完成 + /// + public class PhysicsSimulation + { + public const double EPS = 1e-6; + + public int GAUSS_SEIDEL_ITERS + { + get => 8; + } + + private List _objects; + private LinkedList _objectFreeList; + private int _maximumAllocatedId; + private List _constraints; + private BroadPhase _broadPhase; + private float _gravity; + + private Stopwatch _stopwatchPreIntegration; + private Stopwatch _stopwatchBroadPhase; + private Stopwatch _stopwatchNarrowPhase; + private readonly double _ticksPerMillisecond; + + private long _numBroadPhasePairs; + private long _numNarrowPhasePairs; + + /// + /// 用于性能检测，对各个阶段的耗时进行统计 + /// + public double MeasuredPreIntegrationTimeInMs + { + get => _stopwatchPreIntegration.ElapsedTicks / _ticksPerMillisecond; + } + + public double MeasuredBroadPhaseTimeInMs + { + get => _stopwatchBroadPhase.ElapsedTicks / _ticksPerMillisecond; + } + + public double MeasuredNarrowPhaseTimeInMs + { + get => _stopwatchNarrowPhase.ElapsedTicks / _ticksPerMillisecond; + } + + /// + /// 粗碰撞检测得到的碰撞对数量 + /// + public long NumOfBroadPhasePairs + { + get => _numBroadPhasePairs; + } + + /// + /// 细碰撞检测实际碰撞出现的物体对数量 + /// + public long NumOfNarrowPhasePairs + { + get => _numNarrowPhasePairs; + } + + public float Gravity + { + get => _gravity; + } + + public BroadPhase BroadPhaseCollisionDetector + { + get => _broadPhase; + } + + public PhysicsSimulation() + : this(CollisionGraph.DefaultGraph) + { + } + + public void ClearPhysicsObjects() + { + foreach (PhysicsObject pobj in _objects) + { + if(pobj.IsActive) + { + RemoveObject(pobj); + } + } + } + + public PhysicsSimulation(CollisionGraph graph) + { + _objectFreeList = new LinkedList(); + _objects = new List(); + _maximumAllocatedId = 0; + _constraints = new List(); + _broadPhase = new HashGridMethod(graph); + + _gravity = 9.8f; + + _stopwatchPreIntegration = new Stopwatch(); + _stopwatchBroadPhase = new Stopwatch(); + _stopwatchNarrowPhase = new Stopwatch(); + + _ticksPerMillisecond = Stopwatch.Frequency / 1000.0; + + _numBroadPhasePairs = 0; + _numNarrowPhasePairs = 0; + } + + /// + /// 向世界添加物理对象，如果对象没有刚体部件，那么认为是静态物体 + /// + /// + public void AddPhysicsObject(PhysicsObject pobj) + { + //Main.NewText(pobj.RigidBody.Mass, Color.Red); + pobj.GUID = AllocateGUID(); + if (pobj.GUID == _objects.Count) + { + _objects.Add(pobj); + } + else + { + _objects[pobj.GUID] = pobj; + } + pobj.Initialize(); + } + + /// + /// 向物理世界添加约束对象 + /// + /// + public void AddConstrain(Constraint constrain) + { + _constraints.Add(constrain); + } + + /// + /// Initialize physical object properties + /// + public void Initialize() + { + foreach (PhysicsObject pobj in _objects) + { + pobj.Initialize(); + } + } + + /// + /// 模拟一个完整步长，内部会分为多个子步长 + /// + /// + public void Update(float deltaTime) + { + _stopwatchPreIntegration.Reset(); + _stopwatchBroadPhase.Reset(); + _stopwatchNarrowPhase.Reset(); + CleanUpThisFrame(deltaTime); + float dt = deltaTime / GAUSS_SEIDEL_ITERS; + for (int i = 0; i < GAUSS_SEIDEL_ITERS; i++) + { + _stopwatchPreIntegration.Start(); + PreIntegration(dt); + _stopwatchPreIntegration.Stop(); + + Resolve(dt, _stopwatchBroadPhase, _stopwatchNarrowPhase); + } + + // foreach (var pobj in _objects) + // { + // Main.NewText(pobj.RigidBody.LinearVelocity); + // } + } + + public List<(Vector2, Color)> GetCurrentWireFrames() + { + List<(Vector2, Color)> result = new List<(Vector2, Color)>(); + foreach (PhysicsObject pobj in _objects) + { + if (!pobj.IsActive) + { + continue; + } + + result.AddRange(pobj.GetWireFrameWires()); + } + foreach (Constraint joint in _constraints) + { + result.AddRange(joint.GetDrawMesh()); + } + return result; + } + + //public List GetCustomPhysicsObjects() + //{ + // List result = new List(); + // foreach (PhysicsObject pobj in _objects) + // { + // if (!pobj.IsActive) + // { + // continue; + // } + // if(pobj is CustomPhysicsObject cobj) + // { + // result.Add(cobj); + // } + // } + // return result; + //} + + /// + /// 进行预积分，先模拟一个步长 + /// + /// + private void PreIntegration(float deltaTime) + { + CleanUpThisSubstep(deltaTime); + foreach (PhysicsObject pobj in _objects) + { + if (!pobj.IsActive) + { + continue; + } + pobj.ApplyGravity(new Vector2(0, -_gravity)); + } + foreach (var constrain in _constraints) + { + constrain.ApplyForce(deltaTime); + } + foreach (PhysicsObject pobj in _objects) + { + if (!pobj.IsActive) + { + continue; + } + pobj.RecordOldState(); + pobj.Update(deltaTime); + } + foreach (var constrain in _constraints) + { + constrain.Apply(deltaTime); + } + } + + /// + /// 解算所有物体的约束条件，并且做出响应 + /// + /// + /// + /// + private void Resolve(float deltaTime, Stopwatch broadPhase, Stopwatch narrowPhase) + { + broadPhase.Start(); + List activeObjects = _objects.Where(obj => obj.IsActive).ToList(); + _broadPhase.Prepare(activeObjects, deltaTime); + var pairs = _broadPhase.GetCollisionPairs(deltaTime); + _numBroadPhasePairs = pairs.Count; + broadPhase.Stop(); + + narrowPhase.Start(); + _numNarrowPhasePairs = 0; + List contacts = new List(); + foreach (var pair in pairs) + { + if (pair.Key.TestCollisionCondition(pair.Value, deltaTime, out CollisionInfo info)) + { + // 非线性投影：预先移开以获得contact points + float weightA = info.Source.RigidBody.InvMass / (info.Source.RigidBody.InvMass + info.Target.RigidBody.InvMass); + float weightB = info.Target.RigidBody.InvMass / (info.Source.RigidBody.InvMass + info.Target.RigidBody.InvMass); + + info.Source.RigidBody.MoveBody(info.Normal * weightA * info.Depth, deltaTime); + info.Target.RigidBody.MoveBody(-info.Normal * weightB * info.Depth, deltaTime); + + List events; + pair.Key.GetContactInfo(info, deltaTime, out events); + contacts.AddRange(events); + + //// 非线性投影：回溯，计算实际投影后姿态 + // foreach (var e in events) + // { + // double rAdotN = Vector2.Dot(GeometryUtils.Rotate90(e.LocalOffsetSrc), e.Normal); + // double rBdotN = Vector2.Dot(GeometryUtils.Rotate90(e.LocalOffsetTarget), e.Normal); + // double R1 = rAdotN * rAdotN * e.Source.RigidBody.GlobalInverseInertiaTensor; + // double R2 = rBdotN * rBdotN * e.Target.RigidBody.GlobalInverseInertiaTensor; + // double effectiveMass = R1 + R2 + e.Source.RigidBody.InvMass + e.Target.RigidBody.InvMass; + + // double linearMoveA = info.Depth * e.Source.RigidBody.InvMass / effectiveMass; + // double linearMoveB = -info.Depth * e.Target.RigidBody.InvMass / effectiveMass; + // double angularMoveA = info.Depth * R1 / effectiveMass; + // double angularMoveB = -info.Depth * R2 / effectiveMass; + + // info.Source.RigidBody.MoveBody(info.Normal * (float)linearMoveA, deltaTime); + // info.Target.RigidBody.MoveBody(info.Normal * (float)linearMoveB, deltaTime); + + // double implusePerMoveA = GeometryUtils.Cross(e.LocalOffsetSrc, e.Normal) * e.Source.RigidBody.GlobalInverseInertiaTensor; + // double implusePerMoveB = GeometryUtils.Cross(e.LocalOffsetTarget, e.Normal) * e.Target.RigidBody.GlobalInverseInertiaTensor; + // if (R1 > 0) + // { + // info.Source.Rotation += (float)(angularMoveA * implusePerMoveA / R1); + // } + // if (R2 > 0) + // { + // info.Target.Rotation += (float)(angularMoveB * implusePerMoveB / R2); + // } + // } + + // ApplyConstrains(events, pair.Key.ParentObject, deltaTime); + _numNarrowPhasePairs++; + } + } + + contacts.Sort((a, b) => + { + return -a.Depth.CompareTo(b.Depth); + }); + + foreach (var contact in contacts) + { + float stiffness = Math.Max(0, (contact.Source.RigidBody.Restitution + contact.Target.RigidBody.Restitution) / 2); + SolveContact_Weak(contact, stiffness, true, deltaTime); + } + for (int i = 0; i < 16; i++) + { + bool stable = true; + foreach (var contact in contacts) + { + if (!SolveContact_Weak(contact, 0, false, deltaTime)) + { + stable = false; + } + } + if (stable) + { + break; + } + } + foreach (var contact in contacts) + { + SolveFriction_Weak(contact, deltaTime); + contact.Source.RigidBody.MatchAwakeState(contact.Target.RigidBody, deltaTime); + } + + foreach (PhysicsObject pobj in _objects) + { + if (!pobj.IsActive) + { + continue; + } + pobj.RigidBody.CheckAwake(deltaTime); + } + narrowPhase.Stop(); + } + + private void SolveFriction_Weak(CollisionEvent2D e, float deltaTime) + { + var ri = e.LocalOffsetSrc; + var rb = e.LocalOffsetTarget; + + var va = e.Source.RigidBody.LinearVelocity + GeometryUtils.AngularVelocityToLinearVelocity(ri, e.Source.RigidBody.AngularVelocity); + var vb = e.Target.RigidBody.LinearVelocity + + GeometryUtils.AngularVelocityToLinearVelocity(rb, e.Target.RigidBody.AngularVelocity); + + if (e.NormalVelOld == 0) + { + return; + } + float vrel_n = Vector2.Dot(va - vb, e.Normal); + Vector2 vt = (va - vb) - vrel_n * e.Normal; + + float vel_t = vt.Length(); + float friction = Math.Max(0, (e.Source.RigidBody.Friction + e.Target.RigidBody.Friction) / 2); + + vt = vt.SafeNormalize(Vector2.Zero); + if (vt.Length() == 0) + { + return; + } + double rAdotN = Vector2.Dot(GeometryUtils.Rotate90(ri), vt); + double rBdotN = Vector2.Dot(GeometryUtils.Rotate90(rb), vt); + double R1 = rAdotN * rAdotN * e.Source.RigidBody.GlobalInverseInertiaTensor; // Vector2.Dot(Utils.AnuglarVelocityToLinearVelocity(ri, (float)(GlobalInverseInertiaTensor + + // * Utils.Cross(ri, e.Normal))), e.Normal); + double R2 = rBdotN * rBdotN * e.Target.RigidBody.GlobalInverseInertiaTensor; // Vector2.Dot(Utils.AnuglarVelocityToLinearVelocity(rb, (float)(e.Target.RigidBody.GlobalInverseInertiaTensor + + // * Utils.Cross(rb, e.Normal))), e.Normal); + double effectiveMass = e.Source.RigidBody.InvMass + e.Target.RigidBody.InvMass + R1 + R2; + + // (a × b) × c = (c • a)b - (c • b)a + double J_n = -Math.Min(friction * e.NormalVelOld, vel_t / effectiveMass); + Vector2 J = (float)J_n * vt; // + (float)J_t * va_t_unit; + + // var offset = e.Position - (_globalCentroid + ri); + e.Source.RigidBody.AddImpulseImmediate(J, ri, e.Target.RigidBody, -J, rb); + Debug.Assert(!float.IsNaN(J.X) && !float.IsNaN(J.Y)); + } + + private bool SolveContact_Weak(CollisionEvent2D e, float restitution, bool initial, float deltaTime) + { + var ri = e.LocalOffsetSrc; + var rb = e.LocalOffsetTarget; + + var va = e.Source.RigidBody.LinearVelocity + GeometryUtils.AngularVelocityToLinearVelocity(ri, e.Source.RigidBody.AngularVelocity); + var vb = e.Target.RigidBody.LinearVelocity + + GeometryUtils.AngularVelocityToLinearVelocity(rb, e.Target.RigidBody.AngularVelocity); + + float vrel_n = Vector2.Dot(va - vb, e.Normal); + + if (vrel_n >= 0) + { + return true; + } + + if (initial) + { + if (e.Source.RigidBody.MovementType == MovementType.Player) + { + e.Source.RigidBody.ContactNormals.Add(e.Normal); + } + if (e.Target.RigidBody.MovementType == MovementType.Player) + { + e.Target.RigidBody.ContactNormals.Add(-e.Normal); + } + } + + float vnew_n = restitution * Math.Max(-vrel_n, 0); + + double rAdotN = Vector2.Dot(GeometryUtils.Rotate90(ri), e.Normal); + double rBdotN = Vector2.Dot(GeometryUtils.Rotate90(rb), e.Normal); + double R1 = rAdotN * rAdotN * e.Source.RigidBody.GlobalInverseInertiaTensor; + double R2 = rBdotN * rBdotN * e.Target.RigidBody.GlobalInverseInertiaTensor; + double J_n = (vnew_n - vrel_n) / (e.Source.RigidBody.InvMass + e.Target.RigidBody.InvMass + R1 + R2); + Vector2 J = (float)J_n * e.Normal; // + (float)J_t * va_t_unit; + + e.NormalVelOld += (float)J_n; + + // var offset = e.Position - (_globalCentroid + ri); + e.Source.RigidBody.AddImpulseImmediate(J, ri, e.Target.RigidBody, -J, rb); + Debug.Assert(!float.IsNaN(J.X) && !float.IsNaN(J.Y)); + + return vrel_n > -0.1f; + } + + private Dictionary>> GroupCollisions(List> pairs) + { + Dictionary>> groupedPairs = new Dictionary>>(); + UnionFind uf = new UnionFind(_objects.Count); + foreach (var pair in pairs) + { + uf.Union(pair.Key.ParentObject.GUID, pair.Value.ParentObject.GUID); + } + foreach (var pair in pairs) + { + int group = uf.Find(pair.Key.ParentObject.GUID); + if (groupedPairs.ContainsKey(group)) + { + groupedPairs[group].Add(pair); + } + else + { + groupedPairs.Add(group, new List>() { pair }); + } + } + return groupedPairs; + } + + private void ApplyConstrains(List events, PhysicsObject pobj, float deltaTime) + { + if (deltaTime == 0) + { + return; + } + pobj.RigidBody.RespondToEvents(events, deltaTime); + } + + private void CleanUpThisFrame(float deltaTime) + { + foreach (PhysicsObject pobj in _objects) + { + pobj.CleanThisFrame(deltaTime); + } + } + + private void CleanUpThisSubstep(float deltaTime) + { + foreach (PhysicsObject pobj in _objects) + { + if (!pobj.IsActive) + { + continue; + } + pobj.CleanThisSubstep(deltaTime); + } + } + + public void RemoveObject(PhysicsObject obj) + { + _objectFreeList.AddLast(obj.GUID); + obj.IsActive = false; + } + + private void RemoveObject(int GUID) + { + Debug.Assert(_objects[GUID].IsActive); + RemoveObject(_objects[GUID]); + } + + private List AllocateGUIDs(int size) + { + List results = new List(); + while (size > 0) + { + if (_objectFreeList.Count > 0) + { + results.Add(_objectFreeList.First()); + _objectFreeList.RemoveFirst(); + } + else + { + results.Add(_maximumAllocatedId++); + } + } + return results; + } + + private int AllocateGUID() + { + if (_objectFreeList.Count == 0) + { + return _maximumAllocatedId++; + } + int id = _objectFreeList.First(); + _objectFreeList.RemoveFirst(); + return id; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/PlayGround/Constrain/JointPlayground.cs b/Sources/Everglow.Function/Physics/PBEngine/PlayGround/Constrain/JointPlayground.cs new file mode 100644 index 000000000..eeb4dd3c7 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/PlayGround/Constrain/JointPlayground.cs @@ -0,0 +1,222 @@ +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using Everglow.Commons.Physics.PBEngine.Constraints; +using Everglow.Commons.Physics.PBEngine.Core; + +namespace Everglow.Commons.Physics.PBEngine.PlayGround.Constrain +{ + /// + /// 储存了一些测试约束的场景 + /// + public class JointPlayground + { + public static PhysicsSimulation SimpleJoint() + { + var world = new PhysicsSimulation(); + + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 64), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(500, 660); + dynamicBox.Rotation = 0.0f; + world.AddPhysicsObject(dynamicBox); + + var staticPlane1 = new PhysicsObject( + new BoxCollider(300, 32), null); + staticPlane1.Position = new Vector2(512, 700); + staticPlane1.Rotation = 0f; + world.AddPhysicsObject(staticPlane1); + + var joint = new JointConstraint(staticPlane1, dynamicBox, new Vector2(0, -40), new Vector2(0, 64)); + world.AddConstrain(joint); + + return world; + } + + public static PhysicsSimulation SimpleJointMultiple() + { + var world = new PhysicsSimulation(); + + var staticPlane1 = new PhysicsObject( + new BoxCollider(32, 32), null); + staticPlane1.Position = new Vector2(512, 700); + staticPlane1.Rotation = 0f; + world.AddPhysicsObject(staticPlane1); + + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 64), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(500, 600); + dynamicBox.Rotation = 0.0f; + world.AddPhysicsObject(dynamicBox); + + var dynamicBox1 = new PhysicsObject( + new BoxCollider(32, 64), new RigidBody2D(256)); + dynamicBox1.Position = new Vector2(500, 500); + dynamicBox1.Rotation = 0.0f; + world.AddPhysicsObject(dynamicBox1); + + var joint = new JointConstraint(staticPlane1, dynamicBox, new Vector2(0, -50), new Vector2(0, 50)); + world.AddConstrain(joint); + + var joint2 = new JointConstraint(dynamicBox, dynamicBox1, new Vector2(0, -70), new Vector2(0, 70)); + world.AddConstrain(joint2); + + // var kPlane = new PhysicsObject( + // new BoxCollider(200, 32), null); + // kPlane.Position = new Vector2(512, 400); + // kPlane.Rotation = 0; + // kPlane.RigidBody.ApplyAngularVelocity(0.9f); + // kPlane.RigidBody.MovementType = Collision.MovementType.Kinematic; + // world.AddPhysicsObject(kPlane); + return world; + } + + public static PhysicsSimulation DynamicJoints() + { + var world = new PhysicsSimulation(); + + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 64), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(500, 500); + dynamicBox.Rotation = 0.2f; + world.AddPhysicsObject(dynamicBox); + + var dynamicBox1 = new PhysicsObject( + new BoxCollider(32, 64), new RigidBody2D(256)); + dynamicBox1.Position = new Vector2(500, 440); + dynamicBox1.Rotation = 0.3f; + world.AddPhysicsObject(dynamicBox1); + + var joint2 = new JointConstraint(dynamicBox, dynamicBox1, new Vector2(0, -50), new Vector2(0, 50)); + world.AddConstrain(joint2); + + var staticPlane1 = new PhysicsObject( + new BoxCollider(700, 32), null); + staticPlane1.Position = new Vector2(512, 300); + staticPlane1.Rotation = 0f; + world.AddPhysicsObject(staticPlane1); + + return world; + } + + public static PhysicsSimulation NewtonPendulum() + { + var world = new PhysicsSimulation(); + + var staticPlane1 = new PhysicsObject( + new BoxCollider(512, 32), null); + staticPlane1.Position = new Vector2(512, 800); + staticPlane1.Rotation = 0f; + world.AddPhysicsObject(staticPlane1); + + for (int i = 0; i < 5; i++) + { + var dynamicBox = new PhysicsObject( + new SphereCollider(32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512 + 64 * (i - 2), 600); + if(i == 4) + { + dynamicBox.Position += new Vector2(60, 60); + } + dynamicBox.Rotation = 0.0f; + world.AddPhysicsObject(dynamicBox); + + if (i == 0) + { + dynamicBox.RigidBody.LinearVelocity = new Vector2(-50, 0); + } + + var joint1 = new JointConstraint(staticPlane1, dynamicBox, new Vector2(64 * (i - 2), -16), new Vector2(0, 200)); + world.AddConstrain(joint1); + } + + // var dynamicBox1 = new PhysicsObject( + // new SphereCollider(32), new RigidBody2D(256)); + // dynamicBox1.Position = new Vector2(512, 584); + // dynamicBox1.Rotation = 0.0f; + // world.AddPhysicsObject(dynamicBox1); + + // var dynamicBox2 = new PhysicsObject( + // new SphereCollider(32), new RigidBody2D(256)); + // dynamicBox2.Position = new Vector2(576, 584); + // dynamicBox2.Rotation = 0.0f; + // world.AddPhysicsObject(dynamicBox2); + + // var joint2 = new Joint(staticPlane1, dynamicBox1, new Vector2(0, -16), new Vector2(0, 200)); + // world.AddConstrain(joint2); + + // var joint3 = new Joint(staticPlane1, dynamicBox2, new Vector2(64, -16), new Vector2(0, 200)); + // world.AddConstrain(joint3); + + return world; + } + + public static PhysicsSimulation SimpleSpring() + { + var world = new PhysicsSimulation(); + + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 64), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(300, 100); + dynamicBox.Rotation = 0.0f; + world.AddPhysicsObject(dynamicBox); + + var staticPlane1 = new PhysicsObject( + new BoxCollider(300, 32), null); + staticPlane1.Position = new Vector2(512, 700); + staticPlane1.Rotation = 0f; + world.AddPhysicsObject(staticPlane1); + + var joint = new SpringConstraint(staticPlane1, dynamicBox, 29f, 200); + world.AddConstrain(joint); + + var dynamicBox1 = new PhysicsObject( + new BoxCollider(32, 64), new RigidBody2D(256)); + dynamicBox1.Position = new Vector2(400, 10); + dynamicBox1.Rotation = 0.0f; + world.AddPhysicsObject(dynamicBox1); + + var joint1 = new SpringConstraint(dynamicBox, dynamicBox1, 29f, 200); + world.AddConstrain(joint1); + + return world; + } + + public static PhysicsSimulation SpringTriganluar() + { + var world = new PhysicsSimulation(); + + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 64), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(300, 100); + dynamicBox.Rotation = 0.0f; + world.AddPhysicsObject(dynamicBox); + + var staticPlane1 = new PhysicsObject( + new BoxCollider(32, 32), null); + staticPlane1.Position = new Vector2(512, 700); + staticPlane1.Rotation = 0f; + world.AddPhysicsObject(staticPlane1); + + var staticPlane2 = new PhysicsObject( + new BoxCollider(32, 32), null); + staticPlane2.Position = new Vector2(212, 300); + staticPlane2.Rotation = 0f; + world.AddPhysicsObject(staticPlane2); + + var staticPlane3 = new PhysicsObject( + new BoxCollider(32, 32), null); + staticPlane3.Position = new Vector2(812, 300); + staticPlane3.Rotation = 0f; + world.AddPhysicsObject(staticPlane3); + + var joint = new SpringConstraint(staticPlane1, dynamicBox, 29f, 200); + world.AddConstrain(joint); + var joint1 = new SpringConstraint(staticPlane2, dynamicBox, 29f, 200); + world.AddConstrain(joint1); + + var joint2 = new SpringConstraint(staticPlane3, dynamicBox, 29f, 200); + world.AddConstrain(joint2); + + return world; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/PlayGround/Contact/ContactPlayGround1.cs b/Sources/Everglow.Function/Physics/PBEngine/PlayGround/Contact/ContactPlayGround1.cs new file mode 100644 index 000000000..f0a50b526 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/PlayGround/Contact/ContactPlayGround1.cs @@ -0,0 +1,342 @@ +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using Everglow.Commons.Physics.PBEngine.Core; + +namespace Everglow.Commons.Physics.PBEngine.PlayGround.Contact +{ + /// + /// 储存了一些物理引擎接触测试的场景 + /// + public class ContactPlayGround1 + { + public static PhysicsSimulation SimpleGround1() + { + var world = new PhysicsSimulation(); + + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512, 800); + world.AddPhysicsObject(dynamicBox); + + var staticPlane1 = new PhysicsObject( + new BoxCollider(300, 32), null); + staticPlane1.Position = new Vector2(512, 200); + staticPlane1.Rotation = 0f; + world.AddPhysicsObject(staticPlane1); + + return world; + } + + public static PhysicsSimulation InclinedGround() + { + var world = new PhysicsSimulation(); + + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512, 800); + world.AddPhysicsObject(dynamicBox); + + var staticPlane1 = new PhysicsObject( + new BoxCollider(600, 32), null); + staticPlane1.Position = new Vector2(512, 200); + staticPlane1.Rotation = 0.22f; + world.AddPhysicsObject(staticPlane1); + + return world; + } + + public static PhysicsSimulation MultipleInclinedGround() + { + var world = new PhysicsSimulation(); + + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512, 900); + world.AddPhysicsObject(dynamicBox); + + var staticPlane1 = new PhysicsObject( + new BoxCollider(500, 32), null); + staticPlane1.Position = new Vector2(300, 700); + staticPlane1.Rotation = -0.32f; + world.AddPhysicsObject(staticPlane1); + + var staticPlane2 = new PhysicsObject( + new BoxCollider(500, 32), null); + staticPlane2.Position = new Vector2(680, 200); + staticPlane2.Rotation = 0.32f; + world.AddPhysicsObject(staticPlane2); + + return world; + } + + public static PhysicsSimulation MultipleInclinedGround2() + { + var world = new PhysicsSimulation(); + + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512, 900); + world.AddPhysicsObject(dynamicBox); + + for (int i = 0; i < 5; i++) + { + var staticPlane1 = new PhysicsObject( + new BoxCollider(500, 32), null); + staticPlane1.Position = new Vector2(300, 700 - i * 200); + staticPlane1.Rotation = -0.32f; + world.AddPhysicsObject(staticPlane1); + + var staticPlane2 = new PhysicsObject( + new BoxCollider(500, 32), null); + staticPlane2.Position = new Vector2(680, 700 - i * 200 - 100); + staticPlane2.Rotation = 0.32f; + world.AddPhysicsObject(staticPlane2); + } + return world; + } + + public static PhysicsSimulation MultipleInclinedGround3() + { + var world = new PhysicsSimulation(); + + for (int i = 0; i < 20; i++) + { + var dynamicBox = new PhysicsObject( + new SphereCollider(16), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512 + (i % 10) * 40, 900 + i / 10 * 40); + world.AddPhysicsObject(dynamicBox); + } + + for (int i = 0; i < 5; i++) + { + var staticPlane1 = new PhysicsObject( + new BoxCollider(500, 32), null); + staticPlane1.Position = new Vector2(300, 700 - i * 250); + staticPlane1.Rotation = -0.38f; + world.AddPhysicsObject(staticPlane1); + + var staticPlane2 = new PhysicsObject( + new BoxCollider(500, 32), null); + staticPlane2.Position = new Vector2(680, 700 - i * 250 - 125); + staticPlane2.Rotation = 0.38f; + world.AddPhysicsObject(staticPlane2); + } + return world; + } + + public static PhysicsSimulation MultipleInclinedGroundSphere() + { + var world = new PhysicsSimulation(); + + for (int i = 0; i < 20; i++) + { + var dynamicBall = new PhysicsObject( + new SphereCollider(16), new RigidBody2D(256)); + dynamicBall.Position = new Vector2(512, 800 + i * 40); + world.AddPhysicsObject(dynamicBall); + } + + var staticPlane1 = new PhysicsObject( + new BoxCollider(400, 32), null); + staticPlane1.Position = new Vector2(200, 400); + staticPlane1.Rotation = -0.78f; + world.AddPhysicsObject(staticPlane1); + + var staticPlane2 = new PhysicsObject( + new BoxCollider(400, 32), null); + staticPlane2.Position = new Vector2(400, 400); + staticPlane2.Rotation = 0.78f; + world.AddPhysicsObject(staticPlane2); + + return world; + } + + public static PhysicsSimulation DoubleSlope() + { + var world = new PhysicsSimulation(); + + var dynamicBox = new PhysicsObject( + new BoxCollider(162, 32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(300, 600); + world.AddPhysicsObject(dynamicBox); + + var dynamicBox1 = new PhysicsObject( + new BoxCollider(32, 32), new RigidBody2D(256)); + dynamicBox1.Position = new Vector2(300, 760); + dynamicBox1.RigidBody.ApplyForce(new Vector2(100, 0)); + world.AddPhysicsObject(dynamicBox1); + + for (int i = 0; i < 100; i++) + { + var dynamicBox2 = new PhysicsObject( + new BoxCollider(55, 32), new RigidBody2D(256)); + dynamicBox2.Position = new Vector2(350, 700 + 36 * i); + dynamicBox2.RigidBody.ApplyForce(new Vector2(100, 0)); + world.AddPhysicsObject(dynamicBox2); + } + + var staticPlane1 = new PhysicsObject( + new BoxCollider(400, 32), null); + staticPlane1.Position = new Vector2(200, 400); + staticPlane1.Rotation = -0.78f; + world.AddPhysicsObject(staticPlane1); + + var staticPlane2 = new PhysicsObject( + new BoxCollider(400, 32), null); + staticPlane2.Position = new Vector2(400, 400); + staticPlane2.Rotation = 0.78f; + world.AddPhysicsObject(staticPlane2); + + return world; + } + + public static PhysicsSimulation SimpleKinematic() + { + var world = new PhysicsSimulation(); + + for (int i = 0; i < 200; i++) + { + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512 + (i % 10) * 35, 900 + i / 10 * 35); + world.AddPhysicsObject(dynamicBox); + } + + var kPlane = new PhysicsObject( + new BoxCollider(500, 32), null); + kPlane.Position = new Vector2(512, 512); + kPlane.Rotation = 0; + kPlane.RigidBody.ApplyAngularVelocity(0.9f); + kPlane.RigidBody.MovementType = MovementType.Kinematic; + world.AddPhysicsObject(kPlane); + + var sPlane = new PhysicsObject( + new BoxCollider(32, 600), null); + sPlane.Position = new Vector2(0, 512); + sPlane.Rotation = 0; + world.AddPhysicsObject(sPlane); + + var sPlane2 = new PhysicsObject( + new BoxCollider(32, 600), null); + sPlane2.Position = new Vector2(1024, 512); + sPlane2.Rotation = 0; + world.AddPhysicsObject(sPlane2); + + var sPlane1 = new PhysicsObject( + new BoxCollider(1024, 32), null); + sPlane1.Position = new Vector2(512, 0); + sPlane1.Rotation = 0; + world.AddPhysicsObject(sPlane1); + return world; + } + + public static PhysicsSimulation SimpleSphere() + { + var world = new PhysicsSimulation(); + + var dynamicBox = new PhysicsObject( + new SphereCollider(32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512, 800); + world.AddPhysicsObject(dynamicBox); + + var staticPlane1 = new PhysicsObject( + new BoxCollider(300, 32), null); + staticPlane1.Position = new Vector2(512, 200); + staticPlane1.Rotation = 0f; + world.AddPhysicsObject(staticPlane1); + return world; + } + + public static PhysicsSimulation SlopeSphere() + { + var world = new PhysicsSimulation(); + + var dynamicBox = new PhysicsObject( + new SphereCollider(32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512, 800); + world.AddPhysicsObject(dynamicBox); + + var staticPlane1 = new PhysicsObject( + new BoxCollider(300, 32), null); + staticPlane1.Position = new Vector2(512, 200); + staticPlane1.Rotation = 0.3f; + world.AddPhysicsObject(staticPlane1); + return world; + } + + public static PhysicsSimulation SphereAndBox() + { + var world = new PhysicsSimulation(); + + for (int i = 0; i < 10; i++) + { + var dynamicBox = new PhysicsObject( + new SphereCollider(32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512 + (i - 5) * 50, 800); + world.AddPhysicsObject(dynamicBox); + } + + for (int i = 0; i < 10; i++) + { + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512 + (i - 5) * 50, 700); + world.AddPhysicsObject(dynamicBox); + } + + var staticPlane1 = new PhysicsObject( + new BoxCollider(600, 32), null); + staticPlane1.Position = new Vector2(512, 200); + staticPlane1.Rotation = 0f; + world.AddPhysicsObject(staticPlane1); + return world; + } + + public static PhysicsSimulation Box2() + { + var world = new PhysicsSimulation(); + + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512, 700); + dynamicBox.RigidBody.Restitution = 0.3f; + world.AddPhysicsObject(dynamicBox); + + var dynamicBox1 = new PhysicsObject( + new BoxCollider(32, 32), new RigidBody2D(256)); + dynamicBox1.Position = new Vector2(512, 734); + dynamicBox1.RigidBody.Restitution = 0.3f; + world.AddPhysicsObject(dynamicBox1); + + var staticPlane1 = new PhysicsObject( + new BoxCollider(600, 32), null); + staticPlane1.Position = new Vector2(512, 200); + staticPlane1.Rotation = 0f; + world.AddPhysicsObject(staticPlane1); + return world; + } + + public static PhysicsSimulation BoxStack() + { + var world = new PhysicsSimulation(); + + for (int i = 0; i < 5; i++) + { + for (int j = 1; j <= i + 1; j++) + { + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512 + (j - 2) * 38 - 16 * i, 500 - i * 34); + dynamicBox.RigidBody.Restitution = 0.3f; + world.AddPhysicsObject(dynamicBox); + } + } + + var staticPlane1 = new PhysicsObject( + new BoxCollider(600, 32), null); + staticPlane1.Position = new Vector2(512, 200); + staticPlane1.Rotation = 0f; + world.AddPhysicsObject(staticPlane1); + return world; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/PlayGround/Contact/ContactPlayGround2.cs b/Sources/Everglow.Function/Physics/PBEngine/PlayGround/Contact/ContactPlayGround2.cs new file mode 100644 index 000000000..26c585a64 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/PlayGround/Contact/ContactPlayGround2.cs @@ -0,0 +1,111 @@ +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using Everglow.Commons.Physics.PBEngine.Core; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Everglow.Commons.Physics.PBEngine.PlayGround.Contact +{ + /// + /// 储存了一些物理引擎接触测试的场景 + /// + public class ContactPlayGround2 + { + public static PhysicsSimulation SphereStairCase() + { + var world = new PhysicsSimulation(); + + + var dynamicBox = new PhysicsObject( + new SphereCollider(32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(512, 800); + dynamicBox.RigidBody.LinearVelocity = new Vector2(12, 0); + world.AddPhysicsObject(dynamicBox); + + var staticPlane1 = new PhysicsObject( + new BoxCollider(300, 32), null); + staticPlane1.Position = new Vector2(512, 400); + staticPlane1.Rotation = 0f; + world.AddPhysicsObject(staticPlane1); + + var staticPlane2 = new PhysicsObject( + new BoxCollider(100, 16), null); + staticPlane2.Position = new Vector2(700, 424); + staticPlane2.Rotation = 0f; + world.AddPhysicsObject(staticPlane2); + + var staticPlane3 = new PhysicsObject( + new BoxCollider(100, 16), null); + staticPlane3.Position = new Vector2(400, 424); + staticPlane3.Rotation = 0f; + world.AddPhysicsObject(staticPlane3); + + return world; + } + + public static PhysicsSimulation ExtractionTest() + { + var world = new PhysicsSimulation(); + + + var dynamicBox = new PhysicsObject( + new BoxCollider(128, 32), new RigidBody2D(128)); + dynamicBox.Position = new Vector2(300, 800); + dynamicBox.RigidBody.LinearVelocity = new Vector2(0, 0); + world.AddPhysicsObject(dynamicBox); + + var dynamicball = new PhysicsObject( + new SphereCollider(32), new RigidBody2D(512)); + dynamicball.Position = new Vector2(300, 900); + dynamicball.RigidBody.LinearVelocity = new Vector2(0, 0); + world.AddPhysicsObject(dynamicball); + + var dynamicball2 = new PhysicsObject( + new SphereCollider(32), new RigidBody2D(512)); + dynamicball2.Position = new Vector2(380, 900); + dynamicball2.RigidBody.LinearVelocity = new Vector2(0, 0); + world.AddPhysicsObject(dynamicball2); + + + var staticPlane1 = new PhysicsObject( + new BoxCollider(400, 32), null); + staticPlane1.Position = new Vector2(200, 400); + staticPlane1.Rotation = -0.78f; + world.AddPhysicsObject(staticPlane1); + + + var staticPlane2 = new PhysicsObject( + new BoxCollider(400, 32), null); + staticPlane2.Position = new Vector2(400, 400); + staticPlane2.Rotation = 0.78f; + world.AddPhysicsObject(staticPlane2); + + return world; + } + + public static PhysicsSimulation MoveWithPad() + { + var world = new PhysicsSimulation(); + + + var dynamicBox = new PhysicsObject( + new BoxCollider(32, 32), new RigidBody2D(256)); + dynamicBox.Position = new Vector2(400, 700); + dynamicBox.RigidBody.LinearVelocity = new Vector2(0, 0); + world.AddPhysicsObject(dynamicBox); + + var kPlane = new PhysicsObject( + new BoxCollider(500, 32), null); + kPlane.Position = new Vector2(512, 512); + kPlane.Rotation = 0; + kPlane.RigidBody.Drag = 0; + kPlane.RigidBody.LinearVelocity = new Vector2(10, 0); + kPlane.RigidBody.MovementType = MovementType.Kinematic; + world.AddPhysicsObject(kPlane); + + return world; + } + } +} diff --git a/Sources/Everglow.Function/Physics/PBEngine/README.md b/Sources/Everglow.Function/Physics/PBEngine/README.md new file mode 100644 index 000000000..4a9f94d2c --- /dev/null +++ b/Sources/Everglow.Function/Physics/PBEngine/README.md @@ -0,0 +1,14 @@ +# PBEngine 物理引擎 + +## 扩展 +还有几个课题可以做，有需要可以联系我： +[ ] 玩家和刚体交互，尽量少的影响原版运动 +[ ] 用Marching Cubes或者别的算法进行像素画转三角网格碰撞体 +[ ] 任意多边形的三角剖分和高效碰撞检测 +[ ] 更好的优化约束求解顺序来达到更快的收敛速度 +[ ] 更多约束，如关节、胶、鼠标控制等 +[ ] 多接触点更精准的冲量计算 +[ ] 曲线形体的碰撞检测 +[ ] 粗检测阶段性能优化、并行计算 +[ ] 刚体的自动休眠功能 +[ ] 连续碰撞检测功能以防止高速物体异常穿透 diff --git a/Sources/Everglow.Function/Physics/PhysicsPlayer.cs b/Sources/Everglow.Function/Physics/PhysicsPlayer.cs new file mode 100644 index 000000000..fc733bec5 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PhysicsPlayer.cs @@ -0,0 +1,259 @@ +using System.Data; +using Everglow.Commons.Physics.PBEngine; +using Everglow.Commons.Physics.PBEngine.Collision.BroadPhase; +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using Everglow.Commons.Physics.PBEngine.Constraints; +using Everglow.Commons.Physics.PBEngine.Core; +using Everglow.Commons.Physics.PBEngine.GameInteraction; +using Everglow.Commons.Vertex; +using Microsoft.Xna.Framework.Input; +using ReLogic.Content; +using ReLogic.Graphics; +using Terraria.GameContent; + +namespace Everglow.Commons.Physics; + +public class PhysicsPlayer : ModPlayer +{ + private PhysicsObject _movingPanel; + private Vector2 _prevVelocity; + private float _simulationDt = 0.1f; + public Vector2 _extraVelocity; + public bool _shouldStand; + + public override void OnEnterWorld() + { + } + + public override void PostUpdate() + { + Main.LocalPlayer.velocity += _extraVelocity; + _extraVelocity = Vector2.Zero; + if (Main.LocalPlayer.velocity.Y == 0 + && PhysicsWorldSystem.Instance._dummyPlayer.RigidBody.TangentRelativeVelocity.Count > 0) + { + Vector2 tangent = Vector2.Zero; + foreach (var v in PhysicsWorldSystem.Instance._dummyPlayer.RigidBody.TangentRelativeVelocity) + { + tangent += v; + } + tangent /= PhysicsWorldSystem.Instance._dummyPlayer.RigidBody.TangentRelativeVelocity.Count; + _extraVelocity = tangent * _simulationDt; + _extraVelocity = Vector2.Dot(_extraVelocity, new Vector2(1, 0)) * new Vector2(1, 0); + } + } + + public override void PreUpdate() + { + _shouldStand = false; + _prevVelocity = Main.LocalPlayer.velocity; + base.PreUpdate(); + } + + public override void PreUpdateMovement() + { + //// Main.LocalPlayer.Center = Physics.Utils.ConvertToPhysicsSpace(Display.Instance._dummyPlayer.Position); + // Main.LocalPlayer.velocity = GeometryUtils.ConvertToPhysicsSpace(PhysicWorldSystem.Instance._dummyPlayer.RigidBody.LinearVelocity) * dt; + // + // var t = PhysicWorldSystem.Instance._dummyPlayer.RigidBody.TangetRelativeVelocity; + // Main.NewText(Main.LocalPlayer.velocity, Color.GreenYellow); + Player.velocity -= _extraVelocity; + + float dt = 0.1f; + var oldPos = Main.LocalPlayer.Center; + var preVelY = Main.LocalPlayer.velocity.Y; + var preVel = Main.LocalPlayer.velocity; + + var broadPhase = PhysicsWorldSystem.Instance._realSimulation.BroadPhaseCollisionDetector; + + var temporaryObjects = new List(); + + foreach (var proj in Main.projectile) + { + if (proj.active && proj.tileCollide) + { + if (broadPhase.TestSingleCollision(proj.Hitbox.ToAABBPhysSpace().EnLarge(8 * 16), Vector2.Zero, 0, new List() + { + "Default", + })) + { + var obj = new PhysicsObject(new BoxCollider(proj.width, proj.height), new RigidBody2D(64)); + obj.Position = GeometryUtils.ConvertToPhysicsSpace(proj.Center); + obj.RigidBody.LinearVelocity = GeometryUtils.ConvertToPhysicsSpace(proj.velocity / dt); + obj.RigidBody.MovementType = MovementType.Player; + obj.Tag = "Player"; + obj.RigidBody.Drag = 0; + temporaryObjects.Add(obj); + } + } + } + + foreach (var npc in Main.npc) + { + if (npc.active && !npc.noTileCollide) + { + if (broadPhase.TestSingleCollision(npc.Hitbox.ToAABBPhysSpace().EnLarge(8 * 16), Vector2.Zero, 0, new List() + { + "Default", + })) + { + var obj = new PhysicsObject(new BoxCollider(npc.width, npc.height), new RigidBody2D(512)); + obj.Position = GeometryUtils.ConvertToPhysicsSpace(npc.Center); + obj.RigidBody.LinearVelocity = GeometryUtils.ConvertToPhysicsSpace(npc.velocity / dt); + obj.RigidBody.MovementType = MovementType.Player; + obj.RigidBody.Drag = 0; + obj.Tag = "Player"; + temporaryObjects.Add(obj); + } + } + } + + foreach (var obj in temporaryObjects) + { + PhysicsWorldSystem.Instance._realSimulation.AddPhysicsObject(obj); + } + + // Main.NewText(Main.LocalPlayer.velocity, Color.Lime); + PhysicsWorldSystem.Instance._dummyPlayer.RigidBody.LinearVelocity = GeometryUtils.ConvertToPhysicsSpace(Main.LocalPlayer.velocity / dt); + PhysicsWorldSystem.Instance._dummyPlayer.Position = GeometryUtils.ConvertToPhysicsSpace(oldPos); + PhysicsWorldSystem.Instance._realSimulation.Update(dt); + + foreach (var obj in temporaryObjects) + { + PhysicsWorldSystem.Instance._realSimulation.RemoveObject(obj); + } + + // Main.LocalPlayer.Center = GeometryUtils.ConvertToPhysicsSpace(PhysicWorldSystem.Instance._dummyPlayer.RigidBody.CentroidWorldSpace) - player.; + // Main.NewText(Main.LocalPlayer.velocity,Color.Cyan); + + // Find any valid, standable slope + if (PhysicsWorldSystem.Instance._dummyPlayer.RigidBody.ContactNormals.Count > 0) + { + foreach (var v in PhysicsWorldSystem.Instance._dummyPlayer.RigidBody.ContactNormals) + { + if (v.Y > 0.9f) + { + _shouldStand = true; + + // Omit tangent velocity due to gravity + float mg = -PhysicsWorldSystem.Instance._realSimulation.Gravity; + Vector2 velDueG = new Vector2(0, -Player.gravDir * Player.gravity / dt) * dt; + Vector2 normalDir = Vector2.Dot(velDueG, v) * v; + Vector2 tanDir = velDueG - normalDir; + Player.velocity.Y = 0; + break; + } + } + } + + // if (_shouldStand) + // { + // Main.LocalPlayer.velocity = Main.LocalPlayer.velocity - _prevVelocity; + + // } + + // if (t.Length() > 0 && Math.Abs(GeometryUtils.Cross(new Vector2(1, 0), t)) < 0.3f) + // { + // Main.LocalPlayer.velocity.Y = 0; + // } + // Main.LocalPlayer.velocity -= _extraVelocity; + if (_movingPanel != null) + { + if (Main.time % 1200 < 600) + { + _movingPanel.RigidBody.LinearVelocity = new Vector2(-4, 0); + } + else if (Main.time % 1200 >= 600) + { + _movingPanel.RigidBody.LinearVelocity = new Vector2(4, 0); + } + } + + base.PreUpdateMovement(); + } + + public override void UpdateEquips() + { + base.UpdateEquips(); + } + + public override void SetControls() + { + base.SetControls(); + if (Main.mouseLeft && Main.mouseLeftRelease) + { + for (int i = 0; i < 2; i++) + { + // var ball = new PhysicsObject( + // new BoxCollider(32, 32), + // new RigidBody2D(256)); + // PhysicWorldSystem.Instance._realSimulation.AddPhysicsObject(ball); + // ball.Position = GeometryUtils.ConvertToPhysicsSpace(Main.MouseWorld) + new Vector2(i, 0); + // ball.RigidBody.LinearVelocity = new Vector2(0, 0); + } + } + + if (Main.keyState[Keys.T] == KeyState.Down && Main.oldKeyState[Keys.T] == KeyState.Up) + { + var dynamicBox = new PhysicsObject( + new BoxCollider(128, 32), new RigidBody2D(256)); + + // _movingPanel = dynamicBox; + dynamicBox.Position = GeometryUtils.ConvertToPhysicsSpace(Main.LocalPlayer.Center + new Vector2(0, -200)); + dynamicBox.Rotation = 0.0f; + dynamicBox.RigidBody.MovementType = MovementType.Dynamic; + dynamicBox.RigidBody.UseGravity = true; + dynamicBox.RigidBody.Drag = 0.1f; + PhysicsWorldSystem.Instance._realSimulation.AddPhysicsObject(dynamicBox); + + var staticPlane = new PhysicsObject( + new BoxCollider(256, 128), null); + staticPlane.Rotation = 0.3f; + staticPlane.Position = GeometryUtils.ConvertToPhysicsSpace(Main.LocalPlayer.Center + new Vector2(200, -200)); + PhysicsWorldSystem.Instance._realSimulation.AddPhysicsObject(staticPlane); + + // var ball = new PhysicsObject(new BoxCollider(32, 32), + // new RigidBody2D(256)); + // Display.Instance._realSimulation.AddPhysicsObject(ball); + // ball.Position = Physics.Utils.ConvertToPhysicsSpace(Main.LocalPlayer.Center + new Vector2(0, -300)); + var staticPlane1 = new PhysicsObject( + new BoxCollider(128, 32), null); + staticPlane1.Position = GeometryUtils.ConvertToPhysicsSpace(Main.LocalPlayer.Center + new Vector2(-200, -200)); + staticPlane1.Rotation = 0.0f; + staticPlane1.RigidBody.MovementType = MovementType.Static; + staticPlane1.RigidBody.UseGravity = false; + PhysicsWorldSystem.Instance._realSimulation.AddPhysicsObject(staticPlane1); + + // _movingPanel = staticPlane1; + var joint = new SpringConstraint(staticPlane1, dynamicBox, 100f, 144f, new Vector2(64, -16), new Vector2(64, 16)); + PhysicsWorldSystem.Instance._realSimulation.AddConstrain(joint); + var joint11 = new SpringConstraint(staticPlane1, dynamicBox, 100f, 144f, new Vector2(-64, -16), new Vector2(-64, 16)); + PhysicsWorldSystem.Instance._realSimulation.AddConstrain(joint11); + + // var joint2 = new SpringConstrain(staticPlane, dynamicBox, 100f, 144f, new Vector2(-64, 16), new Vector2(64, -16)); + // PhysicWorldSystem.Instance._realSimulation.AddConstrain(joint2); + // var joint22 = new SpringConstrain(staticPlane, dynamicBox, 100f, 144f, new Vector2(-64, -16), new Vector2(64, 16)); + // PhysicWorldSystem.Instance._realSimulation.AddConstrain(joint22); + + // if (Main.rand.NextBool(2)) + // { + // var ball = new PhysicsObject(new BoxCollider(256, 32), + // new RigidBody2D(128)); + // ball.RigidBody.MovementType = Physics.Collision.MovementType.Kinematic; + // ball.RigidBody.UseGravity = false; + // ball.RigidBody.AngularDrag = 0; + // ball.RigidBody.AngularVelocity = 0.9f; + // Display.Instance._realSimulation.AddPhysicsObject(ball); + // ball.Position = Physics.Utils.ConvertToPhysicsSpace(Main.LocalPlayer.Center + new Vector2(0, -100)); + // } + // else + // { + // var ball = new PhysicsObject(new SphereCollider(32), + // new RigidBody2D(256)); + // Display.Instance._realSimulation.AddPhysicsObject(ball); + // ball.Position = Physics.Utils.ConvertToPhysicsSpace(Main.LocalPlayer.Center + new Vector2(100, 0)); + // ball.RigidBody.LinearVelocity = new Vector2(-78, 0); + // } + } + } +} diff --git a/Sources/Everglow.Function/Physics/PhysicsWorldSystem.cs b/Sources/Everglow.Function/Physics/PhysicsWorldSystem.cs new file mode 100644 index 000000000..1f37d0314 --- /dev/null +++ b/Sources/Everglow.Function/Physics/PhysicsWorldSystem.cs @@ -0,0 +1,185 @@ +using Everglow.Commons.Physics.PBEngine; +using Everglow.Commons.Physics.PBEngine.Collision; +using Everglow.Commons.Physics.PBEngine.Collision.BroadPhase.Structure; +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using Everglow.Commons.Physics.PBEngine.Core; +using Everglow.Commons.Physics.PBEngine.GameInteraction; + +namespace Everglow.Commons.Physics; + +public class PhysicsWorldSystem : ModSystem +{ + public PhysicsSimulation _realSimulation; + public static PhysicsWorldSystem Instance; + public PhysicsObject _dummyPlayer; + + public const float Simulation_DeltaTime = 0.1f; + + public override void Load() + { + Instance = this; + ReStart(); + On_Player.PlayerFrame += On_Player_PlayerFrame; + On_Collision.TileCollision += On_Collision_TileCollision; + } + + public override void Unload() + { + Instance = null; + On_Player.PlayerFrame -= On_Player_PlayerFrame; + On_Collision.TileCollision -= On_Collision_TileCollision; + } + + private Vector2 On_Collision_TileCollision(On_Collision.orig_TileCollision orig, Vector2 Position, Vector2 Velocity, int Width, int Height, bool fallThrough, bool fall2, int gravDir) + { + Position = GeometryUtils.ConvertToPhysicsSpace(Position); + Velocity = GeometryUtils.ConvertToPhysicsSpace(Velocity); + var rect = new AABB(Position - new Vector2(0, Height), Position + new Vector2(Width, 0)); + var oldCenter = rect.Center; + + rect = rect.Move(Velocity); + + var collisions = _realSimulation.BroadPhaseCollisionDetector.GetSingleCollision(rect, Vector2.Zero, 0, new List() + { + "Default", + }); + + if (collisions.Count > 0) + { + var dummyCollider = new BoxCollider(Width, Height); + var dummyPObject = new PhysicsObject(dummyCollider, new RigidBody2D(64)); + dummyPObject.OldPosition = rect.Center; + dummyPObject.RigidBody.CentroidWorldSpace = rect.Center; + + foreach (var collider in collisions) + { + CollisionInfo info; + if (dummyCollider.TestCollisionCondition(collider, Simulation_DeltaTime, out info)) + { + float weightA = info.Source.RigidBody.InvMass / (info.Source.RigidBody.InvMass + info.Target.RigidBody.InvMass); + float weightB = info.Target.RigidBody.InvMass / (info.Source.RigidBody.InvMass + info.Target.RigidBody.InvMass); + + // if (weightA != 0 && weightB != 0) + // weightA = weightB = 0.5f; + + // Vector2 impluseVel = Vector2.Zero; + // List events; + // dummyCollider.GetContactInfo(info, Simulation_DeltaTime, out events); + // foreach(var e in events) + // { + // impluseVel += SolveContactImpluse(e, result + velChange, events.Count, Simulation_DeltaTime); + // } + // dummyPObject.RigidBody.MoveBody(info.Normal * weightA * info.Depth, Simulation_DeltaTime); + if (Math.Abs(info.Normal.Y) < 0.77f) + { + dummyPObject.RigidBody.MoveBody(info.Normal * weightA * info.Depth, Simulation_DeltaTime); + } + else + { + dummyPObject.RigidBody.MoveBody(info.Depth / info.Normal.Y * new Vector2(0, 1), Simulation_DeltaTime); + } + + // if (float.IsNaN(dummyPObject.RigidBody.CentroidWorldSpace.X) || float.IsNaN(dummyPObject.RigidBody.CentroidWorldSpace.Y)) + // { + // if (true) + // ; + // } + // extraVel = collider.ParentObject.RigidBody.LinearVelocity * Simulation_DeltaTime; + // var n = Vector2.Dot(result, info.Normal) * info.Normal; + // result = result - n; + // velChange += impluseVel; + } + } + + // Main.NewText(result + dummyCollider.ParentObject.Position - oldCenter, Color.Red); + // var newDir = result + new Vector2(0, d); + Velocity = dummyCollider.ParentObject.Position - oldCenter; + } + + Vector2 result = orig(GeometryUtils.ConvertToPhysicsSpace(Position), GeometryUtils.ConvertToPhysicsSpace(Velocity), Width, Height, fallThrough, fall2, gravDir); + return result; + } + + /// + /// 返回接触响应后的速度的变化量 + /// + /// + /// + /// + /// + private Vector2 SolveContactImpulse(CollisionEvent2D e, Vector2 sourceVel, int count, float deltaTime) + { + float mass = 64; + var ri = e.LocalOffsetSrc; + var rb = e.LocalOffsetTarget; + + var va = sourceVel; + var vb = e.Target.RigidBody.LinearVelocity + + GeometryUtils.AngularVelocityToLinearVelocity(rb, e.Target.RigidBody.AngularVelocity); + + float va_n = Vector2.Dot(va - vb, e.Normal); + if (va_n >= 0) + { + e.NormalVelOld = 0; + return Vector2.Zero; + } + float stiffness = 0; + float vnew_n = stiffness * Math.Max(-va_n - 20 * deltaTime, 0); + + // (a × b) × c = (c • a)b - (c • b)a + double rAdotN = Vector2.Dot(GeometryUtils.Rotate90(ri), e.Normal); + double rBdotN = Vector2.Dot(GeometryUtils.Rotate90(rb), e.Normal); + double R1 = 0; // Vector2.Dot(Utils.AnuglarVelocityToLinearVelocity(ri, (float)(GlobalInverseInertiaTensor + + // * Utils.Cross(ri, e.Normal))), e.Normal); + double R2 = rBdotN * rBdotN * e.Target.RigidBody.GlobalInverseInertiaTensor; // Vector2.Dot(Utils.AnuglarVelocityToLinearVelocity(rb, (float)(e.Target.RigidBody.GlobalInverseInertiaTensor + + // * Utils.Cross(rb, e.Normal))), e.Normal); + double J_n = (vnew_n - va_n) / (1.0 / mass + e.Target.RigidBody.InvMass + R1 + R2); + Vector2 J = (float)J_n * e.Normal / count; // + (float)J_t * va_t_unit; + + return J / mass; + } + + private void On_Player_PlayerFrame(On_Player.orig_PlayerFrame orig, Player self) + { + Main.LocalPlayer.velocity += self.GetModPlayer()._extraVelocity; + + if (self.GetModPlayer()._shouldStand) + { + Main.LocalPlayer.velocity.Y = 0; + } + orig(self); + Main.LocalPlayer.velocity -= self.GetModPlayer()._extraVelocity; + } + + public void ReStart() + { + if(_realSimulation is not null) + { + _realSimulation.ClearPhysicsObjects(); + } + var terrariaCollisionGroup = new CollisionGraph(); + _realSimulation = new PhysicsSimulation(terrariaCollisionGroup); + terrariaCollisionGroup.AddSingleEdge("Default", "Default"); + terrariaCollisionGroup.AddSingleEdge("Default", "Terrain"); + terrariaCollisionGroup.AddDoubleEdge("Player", "Default"); + + var terrain = new PhysicsObject(new TileCollider(), null); + _realSimulation.AddPhysicsObject(terrain); + terrain.Tag = "Terrain"; + terrain.RigidBody.MovementType = MovementType.Static; + + var rigidb = new RigidBody2D(64); + _dummyPlayer = new PhysicsObject(new BoxCollider(20, 42), rigidb); + _dummyPlayer.Tag = "Player"; + rigidb.MovementType = MovementType.Player; + rigidb.UseGravity = false; + rigidb.AngularDrag = 0; + rigidb.Drag = 0; + rigidb.Restitution = -1; + rigidb.Friction = -1; + _realSimulation.AddPhysicsObject(_dummyPlayer); + _realSimulation.Initialize(); + } +} diff --git a/Sources/Everglow.Function/Physics/VisualPhysicsUnit.cs b/Sources/Everglow.Function/Physics/VisualPhysicsUnit.cs new file mode 100644 index 000000000..a9c24e130 --- /dev/null +++ b/Sources/Everglow.Function/Physics/VisualPhysicsUnit.cs @@ -0,0 +1,68 @@ +using Everglow.Commons.Enums; +using Everglow.Commons.Physics.PBEngine.Core; +using Everglow.Commons.VFX; +using Everglow.Commons.VFX.Pipelines; + +namespace Everglow.Commons.Physics; + +[Pipeline(typeof(WCSPipeline))] +public class VisualPhysicsUnit : Visual +{ + public override CodeLayer DrawLayer => CodeLayer.PostDrawTiles; + + public PhysicsObject physicsObject; + + public int Timer = 0; + + public delegate void VPU_Draw(VisualPhysicsUnit vpu); + + public event VPU_Draw CustomDraw; + + public delegate void VPU_Update(VisualPhysicsUnit vpu); + + public event VPU_Update CustomUpdate; + + public override void OnSpawn() + { + base.OnSpawn(); + } + + public override void Update() + { + if (physicsObject is null || !physicsObject.IsActive) + { + Active = false; + return; + } + Timer++; + CustomUpdate?.Invoke(this); + } + + public override void Draw() + { + if (physicsObject is null || !physicsObject.IsActive) + { + Active = false; + return; + } + CustomDraw?.Invoke(this); + } + + public override void Kill() + { + physicsObject.IsActive = false; + base.Kill(); + } + + public void RegisterCustomCode(VPU_Draw vpu_draw, VPU_Update vpu_update) + { + CustomDraw += vpu_draw; + CustomUpdate += vpu_update; + } + + public void UnregisterCustomCode(VPU_Draw vpu_draw, VPU_Update vpu_update) + { + CustomDraw -= vpu_draw; + CustomUpdate -= vpu_update; + } +} diff --git a/Sources/Modules/Example/README.md b/Sources/Modules/Example/README.md new file mode 100644 index 000000000..547ff8512 --- /dev/null +++ b/Sources/Modules/Example/README.md @@ -0,0 +1,2 @@ +# 测试场景说明 +测试场景的文件将用于开发时效果的测试，请在发布前将名字包含Demo的文件注释掉 diff --git a/Sources/Modules/Example/Test/Diamond.png b/Sources/Modules/Example/Test/Diamond.png new file mode 100644 index 000000000..a8f3c8a58 Binary files /dev/null and b/Sources/Modules/Example/Test/Diamond.png differ diff --git a/Sources/Modules/Example/Test/Diamond_Physics_Item.cs b/Sources/Modules/Example/Test/Diamond_Physics_Item.cs new file mode 100644 index 000000000..45f4c5124 --- /dev/null +++ b/Sources/Modules/Example/Test/Diamond_Physics_Item.cs @@ -0,0 +1,87 @@ +using Everglow.Commons.Physics; +using Everglow.Commons.Physics.PBEngine; +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using Everglow.Commons.Physics.PBEngine.Core; +using static Everglow.Commons.VFX.VFXBatchExtension; + +namespace Everglow.Example.Test; + +/// +/// Devs only. +/// +public class Diamond_Physics_Item : ModItem +{ + public override void SetDefaults() + { + Item.useTime = 21; + Item.useAnimation = 21; + } + + public override void HoldItem(Player player) + { + if (Main.mouseLeft && Main.mouseLeftRelease) + { + if (!Collision.IsWorldPointSolid(Main.MouseWorld)) + { + var rigBody = new SphereCollider(12); + + var box = new PhysicsObject( + new SphereCollider(12), + new RigidBody2D(20) { }); + + PhysicsWorldSystem.Instance._realSimulation.AddPhysicsObject(box); + box.Position = GeometryUtils.ConvertToPhysicsSpace(Main.MouseWorld); + box.RigidBody.LinearVelocity = new Vector2(0, 0); + + var vfx = new VisualPhysicsUnit(); + vfx.physicsObject = box; + vfx.Active = true; + vfx.Visible = true; + vfx.RegisterCustomCode(Draw, Update); + Ins.VFXManager.Add(vfx); + } + } + if (Main.mouseRight && Main.mouseRightRelease) + { + PhysicsWorldSystem.Instance.ReStart(); + } + } + + public void Draw(VisualPhysicsUnit vpu) + { + Vector2 pos = vpu.physicsObject.Position; + pos.Y *= -1; + Color c = Lighting.GetColor(pos.ToTileCoordinates()); + c.A = 200; + Texture2D tex = ModAsset.Diamond.Value; + float rot = vpu.physicsObject.Rotation; + Ins.Batch.Draw(tex, pos, null, c, rot, tex.Size() * 0.5f, 1f, 0); + for (int i = 0; i < 9; i++) + { + tex = ModContent.Request(ModAsset.Diamond_Mod + "_glow" + i).Value; + float value = (pos.X + pos.Y) * 0.03f + rot + i; + float valueR = Math.Max(MathF.Sin(value), 0); + float valueG = Math.Max(MathF.Sin(value + 0.5f), 0); + float valueB = Math.Max(MathF.Sin(value + 1f), 0); + valueR = MathF.Pow(valueR, 4); + valueG = MathF.Pow(valueG, 4); + valueB = MathF.Pow(valueB, 4); + Color reflectColor = new Color(valueR, valueG, valueB, 0); + reflectColor = Lighting.GetColor(pos.ToTileCoordinates(), reflectColor) * 2; + reflectColor.A = 0; + if (i == 3 || i == 4) + { + reflectColor *= 0.2f; + } + Ins.Batch.Draw(tex, pos, null, reflectColor, rot, tex.Size() * 0.5f, 1f, 0); + } + } + + public void Update(VisualPhysicsUnit vpu) + { + // if(vpu.Timer > 150) + // { + // vpu.Kill(); + // } + } +} diff --git a/Sources/Modules/Example/Test/Diamond_Physics_Item.png b/Sources/Modules/Example/Test/Diamond_Physics_Item.png new file mode 100644 index 000000000..6c4a00624 Binary files /dev/null and b/Sources/Modules/Example/Test/Diamond_Physics_Item.png differ diff --git a/Sources/Modules/Example/Test/Diamond_glow0.png b/Sources/Modules/Example/Test/Diamond_glow0.png new file mode 100644 index 000000000..daab66538 Binary files /dev/null and b/Sources/Modules/Example/Test/Diamond_glow0.png differ diff --git a/Sources/Modules/Example/Test/Diamond_glow1.png b/Sources/Modules/Example/Test/Diamond_glow1.png new file mode 100644 index 000000000..a5751b9fe Binary files /dev/null and b/Sources/Modules/Example/Test/Diamond_glow1.png differ diff --git a/Sources/Modules/Example/Test/Diamond_glow2.png b/Sources/Modules/Example/Test/Diamond_glow2.png new file mode 100644 index 000000000..a251cd3e3 Binary files /dev/null and b/Sources/Modules/Example/Test/Diamond_glow2.png differ diff --git a/Sources/Modules/Example/Test/Diamond_glow3.png b/Sources/Modules/Example/Test/Diamond_glow3.png new file mode 100644 index 000000000..416d89dbc Binary files /dev/null and b/Sources/Modules/Example/Test/Diamond_glow3.png differ diff --git a/Sources/Modules/Example/Test/Diamond_glow4.png b/Sources/Modules/Example/Test/Diamond_glow4.png new file mode 100644 index 000000000..2d76e8ecd Binary files /dev/null and b/Sources/Modules/Example/Test/Diamond_glow4.png differ diff --git a/Sources/Modules/Example/Test/Diamond_glow5.png b/Sources/Modules/Example/Test/Diamond_glow5.png new file mode 100644 index 000000000..62f127c67 Binary files /dev/null and b/Sources/Modules/Example/Test/Diamond_glow5.png differ diff --git a/Sources/Modules/Example/Test/Diamond_glow6.png b/Sources/Modules/Example/Test/Diamond_glow6.png new file mode 100644 index 000000000..c8aa6f0b0 Binary files /dev/null and b/Sources/Modules/Example/Test/Diamond_glow6.png differ diff --git a/Sources/Modules/Example/Test/Diamond_glow7.png b/Sources/Modules/Example/Test/Diamond_glow7.png new file mode 100644 index 000000000..6ce2ffac1 Binary files /dev/null and b/Sources/Modules/Example/Test/Diamond_glow7.png differ diff --git a/Sources/Modules/Example/Test/Diamond_glow8.png b/Sources/Modules/Example/Test/Diamond_glow8.png new file mode 100644 index 000000000..f970800e4 Binary files /dev/null and b/Sources/Modules/Example/Test/Diamond_glow8.png differ diff --git a/Sources/Modules/Example/Test/MovePlatform.png b/Sources/Modules/Example/Test/MovePlatform.png new file mode 100644 index 000000000..f38af231e Binary files /dev/null and b/Sources/Modules/Example/Test/MovePlatform.png differ diff --git a/Sources/Modules/Example/Test/PhysicsBody_TestSystem.cs b/Sources/Modules/Example/Test/PhysicsBody_TestSystem.cs new file mode 100644 index 000000000..00ad1e810 --- /dev/null +++ b/Sources/Modules/Example/Test/PhysicsBody_TestSystem.cs @@ -0,0 +1,85 @@ +using Everglow.Commons.Physics; +using Everglow.Commons.Physics.PBEngine; +using Everglow.Commons.Physics.PBEngine.Collision.Colliders; +using Everglow.Commons.Physics.PBEngine.Core; +using static Everglow.Commons.VFX.VFXBatchExtension; + +namespace Everglow.Example.Test; + +/// +/// Devs only. +/// +public class PhysicsBody_TestSystem : ModItem +{ + public override void SetDefaults() + { + Item.useTime = 21; + Item.useAnimation = 21; + } + + public int soundID = 0; + + public override void HoldItem(Player player) + { + if (Main.mouseLeft && Main.mouseLeftRelease) + { + if (!Collision.IsWorldPointSolid(Main.MouseWorld)) + { + var rigBody = new RigidBody2D(1) + { + MovementType = MovementType.Kinematic, + UseGravity = false, + }; + var box = new PhysicsObject( + new BoxCollider(192, 16), + null); + + PhysicsWorldSystem.Instance._realSimulation.AddPhysicsObject(box); + box.Position = GeometryUtils.ConvertToPhysicsSpace(Main.MouseWorld); + box.RigidBody.LinearVelocity = new Vector2(0, 0); + + var vfx = new VisualPhysicsUnit(); + vfx.Timer = 0; + vfx.physicsObject = box; + vfx.Active = true; + vfx.Visible = true; + vfx.RegisterCustomCode(Draw, Update); + Ins.VFXManager.Add(vfx); + } + } + } + + public void Draw(VisualPhysicsUnit vpu) + { + Vector2 pos = vpu.physicsObject.Position; + pos.Y *= -1; + Color c = Lighting.GetColor(pos.ToTileCoordinates()); + c.A = 200; + Texture2D tex = ModAsset.MovePlatform.Value; + float rot = vpu.physicsObject.Rotation; + Ins.Batch.Draw(tex, pos, null, c, rot, tex.Size() * 0.5f, 1f, 0); + } + + public void Update(VisualPhysicsUnit vpu) + { + Vector2 vel; + var timer = vpu.Timer % 600; + if (timer < 100) + { + vel = Vector2.zeroVector; + } + else if(timer >= 100 && timer < 300) + { + vel = new Vector2(0, -2); + } + else if (timer >= 300 && timer < 400) + { + vel = Vector2.zeroVector; + } + else + { + vel = new Vector2(0, 2); + } + vpu.physicsObject.Position += vel; + } +} diff --git a/Sources/Modules/Example/Test/PhysicsBody_TestSystem.png b/Sources/Modules/Example/Test/PhysicsBody_TestSystem.png new file mode 100644 index 000000000..ace253d9d Binary files /dev/null and b/Sources/Modules/Example/Test/PhysicsBody_TestSystem.png differ diff --git a/Sources/Modules/Example/VFX/Default.fx b/Sources/Modules/Example/VFX/Default.fx new file mode 100644 index 000000000..1580a57a4 --- /dev/null +++ b/Sources/Modules/Example/VFX/Default.fx @@ -0,0 +1,44 @@ +// 用于线框的绘制 +sampler uImage0 : register(s0); + +float4x4 uTransform; +float uTime; + +struct VSInput +{ + float2 Pos : POSITION0; + float4 Color : COLOR0; + float2 Texcoord : TEXCOORD0; +}; + +struct PSInput +{ + float4 Pos : SV_POSITION; + float4 Color : COLOR0; + float2 Texcoord : TEXCOORD0; +}; + + +PSInput VertexShaderFunction(VSInput input) +{ + PSInput output; + output.Color = input.Color; + output.Texcoord = input.Texcoord; + output.Pos = mul(float4(input.Pos, 0, 1), uTransform); + return output; +} + + +float4 PixelShaderFunction(PSInput input) : COLOR0 +{ + return tex2D(uImage0, input.Texcoord) * input.Color; +} + +technique Technique1 +{ + pass Test + { + VertexShader = compile vs_3_0 VertexShaderFunction(); + PixelShader = compile ps_3_0 PixelShaderFunction(); + } +}