├── Collisions.cs ├── FlatAABB.cs ├── FlatBody.cs ├── FlatMath.cs ├── FlatTransform.cs ├── FlatVector.cs ├── FlatWorld.cs └── LICENSE /Collisions.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace FlatPhysics 4 | { 5 | public static class Collisions 6 | { 7 | public static bool IntersectCirclePolygon(FlatVector circleCenter, float circleRadius, 8 | FlatVector polygonCenter, FlatVector[] vertices, 9 | out FlatVector normal, out float depth) 10 | { 11 | normal = FlatVector.Zero; 12 | depth = float.MaxValue; 13 | 14 | FlatVector axis = FlatVector.Zero; 15 | float axisDepth = 0f; 16 | float minA, maxA, minB, maxB; 17 | 18 | for (int i = 0; i < vertices.Length; i++) 19 | { 20 | FlatVector va = vertices[i]; 21 | FlatVector vb = vertices[(i + 1) % vertices.Length]; 22 | 23 | FlatVector edge = vb - va; 24 | axis = new FlatVector(-edge.Y, edge.X); 25 | axis = FlatMath.Normalize(axis); 26 | 27 | Collisions.ProjectVertices(vertices, axis, out minA, out maxA); 28 | Collisions.ProjectCircle(circleCenter, circleRadius, axis, out minB, out maxB); 29 | 30 | if (minA >= maxB || minB >= maxA) 31 | { 32 | return false; 33 | } 34 | 35 | axisDepth = MathF.Min(maxB - minA, maxA - minB); 36 | 37 | if (axisDepth < depth) 38 | { 39 | depth = axisDepth; 40 | normal = axis; 41 | } 42 | } 43 | 44 | int cpIndex = Collisions.FindClosestPointOnPolygon(circleCenter, vertices); 45 | FlatVector cp = vertices[cpIndex]; 46 | 47 | axis = cp - circleCenter; 48 | axis = FlatMath.Normalize(axis); 49 | 50 | Collisions.ProjectVertices(vertices, axis, out minA, out maxA); 51 | Collisions.ProjectCircle(circleCenter, circleRadius, axis, out minB, out maxB); 52 | 53 | if (minA >= maxB || minB >= maxA) 54 | { 55 | return false; 56 | } 57 | 58 | axisDepth = MathF.Min(maxB - minA, maxA - minB); 59 | 60 | if (axisDepth < depth) 61 | { 62 | depth = axisDepth; 63 | normal = axis; 64 | } 65 | 66 | FlatVector direction = polygonCenter - circleCenter; 67 | 68 | if (FlatMath.Dot(direction, normal) < 0f) 69 | { 70 | normal = -normal; 71 | } 72 | 73 | return true; 74 | } 75 | 76 | 77 | public static bool IntersectCirclePolygon(FlatVector circleCenter, float circleRadius, 78 | FlatVector[] vertices, 79 | out FlatVector normal, out float depth) 80 | { 81 | normal = FlatVector.Zero; 82 | depth = float.MaxValue; 83 | 84 | FlatVector axis = FlatVector.Zero; 85 | float axisDepth = 0f; 86 | float minA, maxA, minB, maxB; 87 | 88 | for (int i = 0; i < vertices.Length; i++) 89 | { 90 | FlatVector va = vertices[i]; 91 | FlatVector vb = vertices[(i + 1) % vertices.Length]; 92 | 93 | FlatVector edge = vb - va; 94 | axis = new FlatVector(-edge.Y, edge.X); 95 | axis = FlatMath.Normalize(axis); 96 | 97 | Collisions.ProjectVertices(vertices, axis, out minA, out maxA); 98 | Collisions.ProjectCircle(circleCenter, circleRadius, axis, out minB, out maxB); 99 | 100 | if (minA >= maxB || minB >= maxA) 101 | { 102 | return false; 103 | } 104 | 105 | axisDepth = MathF.Min(maxB - minA, maxA - minB); 106 | 107 | if (axisDepth < depth) 108 | { 109 | depth = axisDepth; 110 | normal = axis; 111 | } 112 | } 113 | 114 | int cpIndex = Collisions.FindClosestPointOnPolygon(circleCenter, vertices); 115 | FlatVector cp = vertices[cpIndex]; 116 | 117 | axis = cp - circleCenter; 118 | axis = FlatMath.Normalize(axis); 119 | 120 | Collisions.ProjectVertices(vertices, axis, out minA, out maxA); 121 | Collisions.ProjectCircle(circleCenter, circleRadius, axis, out minB, out maxB); 122 | 123 | if (minA >= maxB || minB >= maxA) 124 | { 125 | return false; 126 | } 127 | 128 | axisDepth = MathF.Min(maxB - minA, maxA - minB); 129 | 130 | if (axisDepth < depth) 131 | { 132 | depth = axisDepth; 133 | normal = axis; 134 | } 135 | 136 | FlatVector polygonCenter = Collisions.FindArithmeticMean(vertices); 137 | 138 | FlatVector direction = polygonCenter - circleCenter; 139 | 140 | if (FlatMath.Dot(direction, normal) < 0f) 141 | { 142 | normal = -normal; 143 | } 144 | 145 | return true; 146 | } 147 | 148 | private static int FindClosestPointOnPolygon(FlatVector circleCenter, FlatVector[] vertices) 149 | { 150 | int result = -1; 151 | float minDistance = float.MaxValue; 152 | 153 | for(int i = 0; i < vertices.Length; i++) 154 | { 155 | FlatVector v = vertices[i]; 156 | float distance = FlatMath.Distance(v, circleCenter); 157 | 158 | if(distance < minDistance) 159 | { 160 | minDistance = distance; 161 | result = i; 162 | } 163 | } 164 | 165 | return result; 166 | } 167 | 168 | private static void ProjectCircle(FlatVector center, float radius, FlatVector axis, out float min, out float max) 169 | { 170 | FlatVector direction = FlatMath.Normalize(axis); 171 | FlatVector directionAndRadius = direction * radius; 172 | 173 | FlatVector p1 = center + directionAndRadius; 174 | FlatVector p2 = center - directionAndRadius; 175 | 176 | min = FlatMath.Dot(p1, axis); 177 | max = FlatMath.Dot(p2, axis); 178 | 179 | if(min > max) 180 | { 181 | // swap the min and max values. 182 | float t = min; 183 | min = max; 184 | max = t; 185 | } 186 | } 187 | 188 | public static bool IntersectPolygons(FlatVector centerA, FlatVector[] verticesA, FlatVector centerB, FlatVector[] verticesB, out FlatVector normal, out float depth) 189 | { 190 | normal = FlatVector.Zero; 191 | depth = float.MaxValue; 192 | 193 | for (int i = 0; i < verticesA.Length; i++) 194 | { 195 | FlatVector va = verticesA[i]; 196 | FlatVector vb = verticesA[(i + 1) % verticesA.Length]; 197 | 198 | FlatVector edge = vb - va; 199 | FlatVector axis = new FlatVector(-edge.Y, edge.X); 200 | axis = FlatMath.Normalize(axis); 201 | 202 | Collisions.ProjectVertices(verticesA, axis, out float minA, out float maxA); 203 | Collisions.ProjectVertices(verticesB, axis, out float minB, out float maxB); 204 | 205 | if (minA >= maxB || minB >= maxA) 206 | { 207 | return false; 208 | } 209 | 210 | float axisDepth = MathF.Min(maxB - minA, maxA - minB); 211 | 212 | if (axisDepth < depth) 213 | { 214 | depth = axisDepth; 215 | normal = axis; 216 | } 217 | } 218 | 219 | for (int i = 0; i < verticesB.Length; i++) 220 | { 221 | FlatVector va = verticesB[i]; 222 | FlatVector vb = verticesB[(i + 1) % verticesB.Length]; 223 | 224 | FlatVector edge = vb - va; 225 | FlatVector axis = new FlatVector(-edge.Y, edge.X); 226 | axis = FlatMath.Normalize(axis); 227 | 228 | Collisions.ProjectVertices(verticesA, axis, out float minA, out float maxA); 229 | Collisions.ProjectVertices(verticesB, axis, out float minB, out float maxB); 230 | 231 | if (minA >= maxB || minB >= maxA) 232 | { 233 | return false; 234 | } 235 | 236 | float axisDepth = MathF.Min(maxB - minA, maxA - minB); 237 | 238 | if (axisDepth < depth) 239 | { 240 | depth = axisDepth; 241 | normal = axis; 242 | } 243 | } 244 | 245 | FlatVector direction = centerB - centerA; 246 | 247 | if (FlatMath.Dot(direction, normal) < 0f) 248 | { 249 | normal = -normal; 250 | } 251 | 252 | return true; 253 | } 254 | 255 | public static bool IntersectPolygons(FlatVector[] verticesA, FlatVector[] verticesB, out FlatVector normal, out float depth) 256 | { 257 | normal = FlatVector.Zero; 258 | depth = float.MaxValue; 259 | 260 | for(int i = 0; i < verticesA.Length; i++) 261 | { 262 | FlatVector va = verticesA[i]; 263 | FlatVector vb = verticesA[(i + 1) % verticesA.Length]; 264 | 265 | FlatVector edge = vb - va; 266 | FlatVector axis = new FlatVector(-edge.Y, edge.X); 267 | axis = FlatMath.Normalize(axis); 268 | 269 | Collisions.ProjectVertices(verticesA, axis, out float minA, out float maxA); 270 | Collisions.ProjectVertices(verticesB, axis, out float minB, out float maxB); 271 | 272 | if(minA >= maxB || minB >= maxA) 273 | { 274 | return false; 275 | } 276 | 277 | float axisDepth = MathF.Min(maxB - minA, maxA - minB); 278 | 279 | if(axisDepth < depth) 280 | { 281 | depth = axisDepth; 282 | normal = axis; 283 | } 284 | } 285 | 286 | for (int i = 0; i < verticesB.Length; i++) 287 | { 288 | FlatVector va = verticesB[i]; 289 | FlatVector vb = verticesB[(i + 1) % verticesB.Length]; 290 | 291 | FlatVector edge = vb - va; 292 | FlatVector axis = new FlatVector(-edge.Y, edge.X); 293 | axis = FlatMath.Normalize(axis); 294 | 295 | Collisions.ProjectVertices(verticesA, axis, out float minA, out float maxA); 296 | Collisions.ProjectVertices(verticesB, axis, out float minB, out float maxB); 297 | 298 | if (minA >= maxB || minB >= maxA) 299 | { 300 | return false; 301 | } 302 | 303 | float axisDepth = MathF.Min(maxB - minA, maxA - minB); 304 | 305 | if (axisDepth < depth) 306 | { 307 | depth = axisDepth; 308 | normal = axis; 309 | } 310 | } 311 | 312 | FlatVector centerA = Collisions.FindArithmeticMean(verticesA); 313 | FlatVector centerB = Collisions.FindArithmeticMean(verticesB); 314 | 315 | FlatVector direction = centerB - centerA; 316 | 317 | if(FlatMath.Dot(direction, normal) < 0f) 318 | { 319 | normal = -normal; 320 | } 321 | 322 | return true; 323 | } 324 | 325 | private static FlatVector FindArithmeticMean(FlatVector[] vertices) 326 | { 327 | float sumX = 0f; 328 | float sumY = 0f; 329 | 330 | for(int i = 0; i < vertices.Length; i++) 331 | { 332 | FlatVector v = vertices[i]; 333 | sumX += v.X; 334 | sumY += v.Y; 335 | } 336 | 337 | return new FlatVector(sumX / (float)vertices.Length, sumY / (float)vertices.Length); 338 | } 339 | 340 | private static void ProjectVertices(FlatVector[] vertices, FlatVector axis, out float min, out float max) 341 | { 342 | min = float.MaxValue; 343 | max = float.MinValue; 344 | 345 | for(int i = 0; i < vertices.Length; i++) 346 | { 347 | FlatVector v = vertices[i]; 348 | float proj = FlatMath.Dot(v, axis); 349 | 350 | if(proj < min) { min = proj; } 351 | if(proj > max) { max = proj; } 352 | } 353 | } 354 | 355 | public static bool IntersectCircles( 356 | FlatVector centerA, float radiusA, 357 | FlatVector centerB, float radiusB, 358 | out FlatVector normal, out float depth) 359 | { 360 | normal = FlatVector.Zero; 361 | depth = 0f; 362 | 363 | float distance = FlatMath.Distance(centerA, centerB); 364 | float radii = radiusA + radiusB; 365 | 366 | if(distance >= radii) 367 | { 368 | return false; 369 | } 370 | 371 | normal = FlatMath.Normalize(centerB - centerA); 372 | depth = radii - distance; 373 | 374 | return true; 375 | } 376 | 377 | } 378 | } 379 | -------------------------------------------------------------------------------- /FlatAABB.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace FlatPhysics 4 | { 5 | public readonly struct FlatAABB 6 | { 7 | public readonly FlatVector Min; 8 | public readonly FlatVector Max; 9 | 10 | public FlatAABB(FlatVector min, FlatVector max) 11 | { 12 | this.Min = min; 13 | this.Max = max; 14 | } 15 | 16 | public FlatAABB(float minX, float minY, float maxX, float maxY) 17 | { 18 | this.Min = new FlatVector(minX, minY); 19 | this.Max = new FlatVector(maxX, maxY); 20 | } 21 | } 22 | } 23 | -------------------------------------------------------------------------------- /FlatBody.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace FlatPhysics 4 | { 5 | public enum ShapeType 6 | { 7 | Circle = 0, 8 | Box = 1 9 | } 10 | 11 | public sealed class FlatBody 12 | { 13 | private FlatVector position; 14 | private FlatVector linearVelocity; 15 | private float rotation; 16 | private float rotationalVelocity; 17 | 18 | private FlatVector force; 19 | 20 | public readonly float Density; 21 | public readonly float Mass; 22 | public readonly float InvMass; 23 | public readonly float Restitution; 24 | public readonly float Area; 25 | 26 | public readonly bool IsStatic; 27 | 28 | public readonly float Radius; 29 | public readonly float Width; 30 | public readonly float Height; 31 | 32 | private readonly FlatVector[] vertices; 33 | public readonly int[] Triangles; 34 | private FlatVector[] transformedVertices; 35 | private FlatAABB aabb; 36 | 37 | private bool transformUpdateRequired; 38 | private bool aabbUpdateRequired; 39 | 40 | public readonly ShapeType ShapeType; 41 | 42 | public FlatVector Position 43 | { 44 | get { return this.position; } 45 | } 46 | 47 | public FlatVector LinearVelocity 48 | { 49 | get { return this.linearVelocity; } 50 | internal set { this.linearVelocity = value; } 51 | } 52 | 53 | private FlatBody(FlatVector position, float density, float mass, float restitution, float area, 54 | bool isStatic, float radius, float width, float height, ShapeType shapeType) 55 | { 56 | this.position = position; 57 | this.linearVelocity = FlatVector.Zero; 58 | this.rotation = 0f; 59 | this.rotationalVelocity = 0f; 60 | 61 | this.force = FlatVector.Zero; 62 | 63 | this.Density = density; 64 | this.Mass = mass; 65 | this.Restitution = restitution; 66 | this.Area = area; 67 | 68 | this.IsStatic = isStatic; 69 | this.Radius = radius; 70 | this.Width = width; 71 | this.Height = height; 72 | this.ShapeType = shapeType; 73 | 74 | if(!this.IsStatic) 75 | { 76 | this.InvMass = 1f / this.Mass; 77 | } 78 | else 79 | { 80 | this.InvMass = 0f; 81 | } 82 | 83 | if(this.ShapeType is ShapeType.Box) 84 | { 85 | this.vertices = FlatBody.CreateBoxVertices(this.Width, this.Height); 86 | this.Triangles = FlatBody.CreateBoxTriangles(); 87 | this.transformedVertices = new FlatVector[this.vertices.Length]; 88 | } 89 | else 90 | { 91 | this.vertices = null; 92 | Triangles = null; 93 | this.transformedVertices = null; 94 | } 95 | 96 | this.transformUpdateRequired = true; 97 | this.aabbUpdateRequired = true; 98 | } 99 | 100 | private static FlatVector[] CreateBoxVertices(float width, float height) 101 | { 102 | float left = -width / 2f; 103 | float right = left + width; 104 | float bottom = -height / 2f; 105 | float top = bottom + height; 106 | 107 | FlatVector[] vertices = new FlatVector[4]; 108 | vertices[0] = new FlatVector(left, top); 109 | vertices[1] = new FlatVector(right, top); 110 | vertices[2] = new FlatVector(right, bottom); 111 | vertices[3] = new FlatVector(left, bottom); 112 | 113 | return vertices; 114 | } 115 | 116 | private static int[] CreateBoxTriangles() 117 | { 118 | int[] triangles = new int[6]; 119 | triangles[0] = 0; 120 | triangles[1] = 1; 121 | triangles[2] = 2; 122 | triangles[3] = 0; 123 | triangles[4] = 2; 124 | triangles[5] = 3; 125 | return triangles; 126 | } 127 | 128 | public FlatVector[] GetTransformedVertices() 129 | { 130 | if(this.transformUpdateRequired) 131 | { 132 | FlatTransform transform = new FlatTransform(this.position, this.rotation); 133 | 134 | for(int i = 0; i < this.vertices.Length; i++) 135 | { 136 | FlatVector v = this.vertices[i]; 137 | this.transformedVertices[i] = FlatVector.Transform(v, transform); 138 | } 139 | } 140 | 141 | this.transformUpdateRequired = false; 142 | return this.transformedVertices; 143 | } 144 | 145 | public FlatAABB GetAABB() 146 | { 147 | if (this.aabbUpdateRequired) 148 | { 149 | float minX = float.MaxValue; 150 | float minY = float.MaxValue; 151 | float maxX = float.MinValue; 152 | float maxY = float.MinValue; 153 | 154 | if (this.ShapeType is ShapeType.Box) 155 | { 156 | FlatVector[] vertices = this.GetTransformedVertices(); 157 | 158 | for (int i = 0; i < vertices.Length; i++) 159 | { 160 | FlatVector v = vertices[i]; 161 | 162 | if (v.X < minX) { minX = v.X; } 163 | if (v.X > maxX) { maxX = v.X; } 164 | if (v.Y < minY) { minY = v.Y; } 165 | if (v.Y > maxY) { maxY = v.Y; } 166 | } 167 | } 168 | else if (this.ShapeType is ShapeType.Circle) 169 | { 170 | minX = this.position.X - this.Radius; 171 | minY = this.position.Y - this.Radius; 172 | maxX = this.position.X + this.Radius; 173 | maxY = this.position.Y + this.Radius; 174 | } 175 | else 176 | { 177 | throw new Exception("Unknown ShapeType."); 178 | } 179 | 180 | this.aabb = new FlatAABB(minX, minY, maxX, maxY); 181 | } 182 | 183 | this.aabbUpdateRequired = false; 184 | return this.aabb; 185 | } 186 | 187 | internal void Step(float time, FlatVector gravity, int iterations) 188 | { 189 | if(this.IsStatic) 190 | { 191 | return; 192 | } 193 | 194 | time /= (float)iterations; 195 | 196 | // force = mass * acc 197 | // acc = force / mass; 198 | 199 | //FlatVector acceleration = this.force / this.Mass; 200 | //this.linearVelocity += acceleration * time; 201 | 202 | 203 | this.linearVelocity += gravity * time; 204 | this.position += this.linearVelocity * time; 205 | 206 | this.rotation += this.rotationalVelocity * time; 207 | 208 | this.force = FlatVector.Zero; 209 | this.transformUpdateRequired = true; 210 | this.aabbUpdateRequired = true; 211 | } 212 | 213 | public void Move(FlatVector amount) 214 | { 215 | this.position += amount; 216 | this.transformUpdateRequired = true; 217 | this.aabbUpdateRequired = true; 218 | } 219 | 220 | public void MoveTo(FlatVector position) 221 | { 222 | this.position = position; 223 | this.transformUpdateRequired = true; 224 | this.aabbUpdateRequired = true; 225 | } 226 | 227 | public void Rotate(float amount) 228 | { 229 | this.rotation += amount; 230 | this.transformUpdateRequired = true; 231 | this.aabbUpdateRequired = true; 232 | } 233 | 234 | public void AddForce(FlatVector amount) 235 | { 236 | this.force = amount; 237 | } 238 | 239 | public static bool CreateCircleBody(float radius, FlatVector position, float density, bool isStatic, float restitution, out FlatBody body, out string errorMessage) 240 | { 241 | body = null; 242 | errorMessage = string.Empty; 243 | 244 | float area = radius * radius * MathF.PI; 245 | 246 | if(area < FlatWorld.MinBodySize) 247 | { 248 | errorMessage = $"Circle radius is too small. Min circle area is {FlatWorld.MinBodySize}."; 249 | return false; 250 | } 251 | 252 | if(area > FlatWorld.MaxBodySize) 253 | { 254 | errorMessage = $"Circle radius is too large. Max circle area is {FlatWorld.MaxBodySize}."; 255 | return false; 256 | } 257 | 258 | if (density < FlatWorld.MinDensity) 259 | { 260 | errorMessage = $"Density is too small. Min density is {FlatWorld.MinDensity}"; 261 | return false; 262 | } 263 | 264 | if (density > FlatWorld.MaxDensity) 265 | { 266 | errorMessage = $"Density is too large. Max density is {FlatWorld.MaxDensity}"; 267 | return false; 268 | } 269 | 270 | restitution = FlatMath.Clamp(restitution, 0f, 1f); 271 | 272 | // mass = area * depth * density 273 | float mass = area * density; 274 | 275 | body = new FlatBody(position, density, mass, restitution, area, isStatic, radius, 0f, 0f, ShapeType.Circle); 276 | return true; 277 | } 278 | 279 | public static bool CreateBoxBody(float width, float height, FlatVector position, float density, bool isStatic, float restitution, out FlatBody body, out string errorMessage) 280 | { 281 | body = null; 282 | errorMessage = string.Empty; 283 | 284 | float area = width * height; 285 | 286 | if (area < FlatWorld.MinBodySize) 287 | { 288 | errorMessage = $"Area is too small. Min area is {FlatWorld.MinBodySize}."; 289 | return false; 290 | } 291 | 292 | if (area > FlatWorld.MaxBodySize) 293 | { 294 | errorMessage = $"Area is too large. Max area is {FlatWorld.MaxBodySize}."; 295 | return false; 296 | } 297 | 298 | if (density < FlatWorld.MinDensity) 299 | { 300 | errorMessage = $"Density is too small. Min density is {FlatWorld.MinDensity}"; 301 | return false; 302 | } 303 | 304 | if (density > FlatWorld.MaxDensity) 305 | { 306 | errorMessage = $"Density is too large. Max density is {FlatWorld.MaxDensity}"; 307 | return false; 308 | } 309 | 310 | restitution = FlatMath.Clamp(restitution, 0f, 1f); 311 | 312 | // mass = area * depth * density 313 | float mass = area * density; 314 | 315 | body = new FlatBody(position, density, mass, restitution, area, isStatic, 0f, width, height, ShapeType.Box); 316 | return true; 317 | } 318 | } 319 | } 320 | -------------------------------------------------------------------------------- /FlatMath.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace FlatPhysics 4 | { 5 | public static class FlatMath 6 | { 7 | public static float Clamp(float value, float min, float max) 8 | { 9 | if(min == max) 10 | { 11 | return min; 12 | } 13 | 14 | if(min > max) 15 | { 16 | throw new ArgumentOutOfRangeException("min is greater than the max."); 17 | } 18 | 19 | if(value < min) 20 | { 21 | return min; 22 | } 23 | 24 | if(value > max) 25 | { 26 | return max; 27 | } 28 | 29 | return value; 30 | } 31 | 32 | public static int Clamp(int value, int min, int max) 33 | { 34 | if (min == max) 35 | { 36 | return min; 37 | } 38 | 39 | if (min > max) 40 | { 41 | throw new ArgumentOutOfRangeException("min is greater than the max."); 42 | } 43 | 44 | if (value < min) 45 | { 46 | return min; 47 | } 48 | 49 | if (value > max) 50 | { 51 | return max; 52 | } 53 | 54 | return value; 55 | } 56 | 57 | 58 | public static float Length(FlatVector v) 59 | { 60 | return MathF.Sqrt(v.X * v.X + v.Y * v.Y); 61 | } 62 | 63 | public static float Distance(FlatVector a, FlatVector b) 64 | { 65 | float dx = a.X - b.X; 66 | float dy = a.Y - b.Y; 67 | return MathF.Sqrt(dx * dx + dy * dy); 68 | } 69 | 70 | public static FlatVector Normalize(FlatVector v) 71 | { 72 | float len = FlatMath.Length(v); 73 | return new FlatVector(v.X / len, v.Y / len); 74 | } 75 | 76 | public static float Dot(FlatVector a, FlatVector b) 77 | { 78 | // a · b = ax * bx + ay * by 79 | return a.X * b.X + a.Y * b.Y; 80 | } 81 | 82 | public static float Cross(FlatVector a, FlatVector b) 83 | { 84 | // cz = ax * by − ay * bx 85 | return a.X * b.Y - a.Y * b.X; 86 | } 87 | 88 | } 89 | } 90 | -------------------------------------------------------------------------------- /FlatTransform.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace FlatPhysics 4 | { 5 | internal readonly struct FlatTransform 6 | { 7 | public readonly float PositionX; 8 | public readonly float PositionY; 9 | public readonly float Sin; 10 | public readonly float Cos; 11 | 12 | public readonly static FlatTransform Zero = new FlatTransform(0f, 0f, 0f); 13 | 14 | public FlatTransform(FlatVector position, float angle) 15 | { 16 | this.PositionX = position.X; 17 | this.PositionY = position.Y; 18 | this.Sin = MathF.Sin(angle); 19 | this.Cos = MathF.Cos(angle); 20 | } 21 | 22 | public FlatTransform(float x, float y, float angle) 23 | { 24 | this.PositionX = x; 25 | this.PositionY = y; 26 | this.Sin = MathF.Sin(angle); 27 | this.Cos = MathF.Cos(angle); 28 | } 29 | 30 | 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /FlatVector.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace FlatPhysics 4 | { 5 | public readonly struct FlatVector 6 | { 7 | public readonly float X; 8 | public readonly float Y; 9 | 10 | public static readonly FlatVector Zero = new FlatVector(0f, 0f); 11 | 12 | public FlatVector(float x, float y) 13 | { 14 | this.X = x; 15 | this.Y = y; 16 | } 17 | 18 | public static FlatVector operator +(FlatVector a, FlatVector b) 19 | { 20 | return new FlatVector(a.X + b.X, a.Y + b.Y); 21 | } 22 | 23 | public static FlatVector operator -(FlatVector a, FlatVector b) 24 | { 25 | return new FlatVector(a.X - b.X, a.Y - b.Y); 26 | } 27 | 28 | public static FlatVector operator -(FlatVector v) 29 | { 30 | return new FlatVector(-v.X, -v.Y); 31 | } 32 | 33 | public static FlatVector operator *(FlatVector v, float s) 34 | { 35 | return new FlatVector(v.X * s, v.Y * s); 36 | } 37 | 38 | public static FlatVector operator *(float s, FlatVector v) 39 | { 40 | return new FlatVector(v.X * s, v.Y * s); 41 | } 42 | 43 | public static FlatVector operator /(FlatVector v, float s) 44 | { 45 | return new FlatVector(v.X / s, v.Y / s); 46 | } 47 | 48 | internal static FlatVector Transform(FlatVector v, FlatTransform transform) 49 | { 50 | return new FlatVector( 51 | transform.Cos * v.X - transform.Sin * v.Y + transform.PositionX, 52 | transform.Sin * v.X + transform.Cos * v.Y + transform.PositionY); 53 | } 54 | 55 | public bool Equals(FlatVector other) 56 | { 57 | return this.X == other.X && this.Y == other.Y; 58 | } 59 | 60 | public override bool Equals(object obj) 61 | { 62 | if (obj is FlatVector other) 63 | { 64 | return this.Equals(other); 65 | } 66 | 67 | return false; 68 | } 69 | 70 | public override int GetHashCode() 71 | { 72 | return new { this.X, this.Y }.GetHashCode(); 73 | } 74 | 75 | public override string ToString() 76 | { 77 | return $"X: {this.X}, Y: {this.Y}"; 78 | } 79 | } 80 | } 81 | -------------------------------------------------------------------------------- /FlatWorld.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | 4 | namespace FlatPhysics 5 | { 6 | public sealed class FlatWorld 7 | { 8 | public static readonly float MinBodySize = 0.01f * 0.01f; 9 | public static readonly float MaxBodySize = 64f * 64f; 10 | 11 | public static readonly float MinDensity = 0.5f; // g/cm^3 12 | public static readonly float MaxDensity = 21.4f; 13 | 14 | public static readonly int MinIterations = 1; 15 | public static readonly int MaxIterations = 128; 16 | 17 | private FlatVector gravity; 18 | private List bodyList; 19 | 20 | public int BodyCount 21 | { 22 | get { return this.bodyList.Count; } 23 | } 24 | 25 | public FlatWorld() 26 | { 27 | this.gravity = new FlatVector(0f, -9.81f); 28 | this.bodyList = new List(); 29 | } 30 | 31 | public void AddBody(FlatBody body) 32 | { 33 | this.bodyList.Add(body); 34 | } 35 | 36 | public bool RemoveBody(FlatBody body) 37 | { 38 | return this.bodyList.Remove(body); 39 | } 40 | 41 | public bool GetBody(int index, out FlatBody body) 42 | { 43 | body = null; 44 | 45 | if(index < 0 || index >= this.bodyList.Count) 46 | { 47 | return false; 48 | } 49 | 50 | body = this.bodyList[index]; 51 | return true; 52 | } 53 | 54 | public void Step(float time, int iterations) 55 | { 56 | iterations = FlatMath.Clamp(iterations, FlatWorld.MinIterations, FlatWorld.MaxIterations); 57 | 58 | for (int it = 0; it < iterations; it++) 59 | { 60 | // Movement step 61 | for (int i = 0; i < this.bodyList.Count; i++) 62 | { 63 | this.bodyList[i].Step(time, this.gravity, iterations); 64 | } 65 | 66 | // collision step 67 | for (int i = 0; i < this.bodyList.Count - 1; i++) 68 | { 69 | FlatBody bodyA = this.bodyList[i]; 70 | 71 | for (int j = i + 1; j < this.bodyList.Count; j++) 72 | { 73 | FlatBody bodyB = this.bodyList[j]; 74 | 75 | if (bodyA.IsStatic && bodyB.IsStatic) 76 | { 77 | continue; 78 | } 79 | 80 | if (this.Collide(bodyA, bodyB, out FlatVector normal, out float depth)) 81 | { 82 | if (bodyA.IsStatic) 83 | { 84 | bodyB.Move(normal * depth); 85 | } 86 | else if (bodyB.IsStatic) 87 | { 88 | bodyA.Move(-normal * depth); 89 | } 90 | else 91 | { 92 | bodyA.Move(-normal * depth / 2f); 93 | bodyB.Move(normal * depth / 2f); 94 | } 95 | 96 | this.ResolveCollision(bodyA, bodyB, normal, depth); 97 | } 98 | } 99 | } 100 | } 101 | } 102 | 103 | public void ResolveCollision(FlatBody bodyA, FlatBody bodyB, FlatVector normal, float depth) 104 | { 105 | FlatVector relativeVelocity = bodyB.LinearVelocity - bodyA.LinearVelocity; 106 | 107 | if (FlatMath.Dot(relativeVelocity, normal) > 0f) 108 | { 109 | return; 110 | } 111 | 112 | float e = MathF.Min(bodyA.Restitution, bodyB.Restitution); 113 | 114 | float j = -(1f + e) * FlatMath.Dot(relativeVelocity, normal); 115 | j /= bodyA.InvMass + bodyB.InvMass; 116 | 117 | FlatVector impulse = j * normal; 118 | 119 | bodyA.LinearVelocity -= impulse * bodyA.InvMass; 120 | bodyB.LinearVelocity += impulse * bodyB.InvMass; 121 | } 122 | 123 | public bool Collide(FlatBody bodyA, FlatBody bodyB, out FlatVector normal, out float depth) 124 | { 125 | normal = FlatVector.Zero; 126 | depth = 0f; 127 | 128 | ShapeType shapeTypeA = bodyA.ShapeType; 129 | ShapeType shapeTypeB = bodyB.ShapeType; 130 | 131 | if(shapeTypeA is ShapeType.Box) 132 | { 133 | if (shapeTypeB is ShapeType.Box) 134 | { 135 | return Collisions.IntersectPolygons( 136 | bodyA.Position, bodyA.GetTransformedVertices(), 137 | bodyB.Position, bodyB.GetTransformedVertices(), 138 | out normal, out depth); 139 | } 140 | else if (shapeTypeB is ShapeType.Circle) 141 | { 142 | bool result = Collisions.IntersectCirclePolygon( 143 | bodyB.Position, bodyB.Radius, 144 | bodyA.Position, bodyA.GetTransformedVertices(), 145 | out normal, out depth); 146 | 147 | normal = -normal; 148 | return result; 149 | } 150 | } 151 | else if(shapeTypeA is ShapeType.Circle) 152 | { 153 | if (shapeTypeB is ShapeType.Box) 154 | { 155 | return Collisions.IntersectCirclePolygon( 156 | bodyA.Position, bodyA.Radius, 157 | bodyB.Position, bodyB.GetTransformedVertices(), 158 | out normal, out depth); 159 | } 160 | else if (shapeTypeB is ShapeType.Circle) 161 | { 162 | return Collisions.IntersectCircles( 163 | bodyA.Position, bodyA.Radius, 164 | bodyB.Position, bodyB.Radius, 165 | out normal, out depth); 166 | } 167 | } 168 | 169 | return false; 170 | } 171 | } 172 | } 173 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 twobitcoder101 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | --------------------------------------------------------------------------------