From fa05ad4498b0c48ebc2cf279e7424cb5e878eda7 Mon Sep 17 00:00:00 2001 From: Milan Nikolic Date: Tue, 2 May 2017 21:49:12 +0200 Subject: [PATCH] Physics - WIP --- physics/physics.go | 1249 ++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 1191 insertions(+), 58 deletions(-) diff --git a/physics/physics.go b/physics/physics.go index bb7cc41..605e24e 100644 --- a/physics/physics.go +++ b/physics/physics.go @@ -4,69 +4,52 @@ package physics import ( + "fmt" + "math" + "github.com/gen2brain/raylib-go/raylib" -) - -// Defines -const ( - PhysacMaxBodies = 64 - PhysacMaxManifolds = 4096 - PhysacMaxVertices = 24 - PhysacCircleVertices = 24 - - PhysacDesiredDeltatime = 1.0 / 60.0 - PhysacMaxTimestep = 0.02 - PhysacCollisionIterations = 100 - PhysacPenetrationAllowance = 0.05 - PhysacPenetrationCorrection = 0.4 + "github.com/gen2brain/raylib-go/raymath" ) // Physics shape type -const ( - PhysicsCircle = iota - PhysicsPolygon -) +type ShapeType int -// Mat2 type (used for polygon shape rotation matrix) -type Mat2 struct { - M00 float32 - M01 float32 - M10 float32 - M11 float32 -} +// Physics shape types +const ( + // Circle type + Circle ShapeType = 1 + // Polygon type + Polygon ShapeType = 2 +) // PolygonData type type PolygonData struct { // Current used vertex and normals count - VertexCount uint32 + VertexCount int // Polygon vertex positions vectors - Vertices [24]raylib.Vector2 + Vertices [maxVertices]raylib.Vector2 // Polygon vertex normals vectors - Normals [24]raylib.Vector2 + Normals [maxVertices]raylib.Vector2 // Vertices transform matrix 2x2 - Transform Mat2 + Transform raylib.Mat2 } -// PhysicsShape type -type PhysicsShape struct { +// Shape type +type Shape struct { // Physics shape type (circle or polygon) - Type uint32 - // Padding - _ [4]byte + Type ShapeType // Shape physics body reference - Body *PhysicsBodyData + Body *Body // Circle shape radius (used for circle shapes) Radius float32 // Polygon shape vertices position and normals data (just used for polygon shapes) - VertexData PolygonData + VertexData *PolygonData } -// PhysicsBodyData type -type PhysicsBodyData struct { - // Reference unique identifier - Id uint32 +// Body type +type Body struct { // Enabled dynamics state (collisions are calculated anyway) - Enabled uint32 + Enabled bool // Physics body shape pivot Position raylib.Vector2 // Current linear velocity applied to position @@ -94,27 +77,21 @@ type PhysicsBodyData struct { // Restitution coefficient of the body (0 to 1) Restitution float32 // Apply gravity force to dynamics - UseGravity uint32 + UseGravity bool // Physics grounded on other body state - IsGrounded uint32 + IsGrounded bool // Physics rotation constraint - FreezeOrient uint32 - // Padding - _ [4]byte + FreezeOrient bool // Physics body shape information (type, radius, vertices, normals) - Shape PhysicsShape + Shape *Shape } -// PhysicsManifoldData type -type PhysicsManifoldData struct { - // Reference unique identifier - Id uint32 - // Paddin - _ [4]byte +// Manifold type +type Manifold struct { // Manifold first physics body reference - BodyA *PhysicsBodyData + BodyA *Body // Manifold second physics body reference - BodyB *PhysicsBodyData + BodyB *Body // Depth of penetration from collision Penetration float32 // Normal direction vector from 'a' to 'b' @@ -122,13 +99,1169 @@ type PhysicsManifoldData struct { // Points of contact during collision Contacts [2]raylib.Vector2 // Current collision number of contacts - ContactsCount uint32 + ContactsCount int // Mixed restitution during collision Restitution float32 // Mixed dynamic friction during collision DynamicFriction float32 // Mixed static friction during collision StaticFriction float32 - // Padding - _ [4]byte +} + +// Defines +const ( + maxVertices = 24 + circleVertices = 24 + + collisionIterations = 100 + penetrationAllowance = 0.05 + penetrationCorrection = 0.4 + + fltMax = 3.402823466 + 38 + epsilon = 0.000001 +) + +var ( + // Physics world gravity force + gravityForce raylib.Vector2 + + // Physics bodies pointers array + bodies []*Body + + // Physics world current bodies counter + bodiesCount int + + // Physics manifolds pointers array + manifolds []*Manifold + + // Physics world current manifolds counter + manifoldsCount int + + // Delta time used for physics steps + deltaTime float32 +) + +// Init - initializes physics values +func Init() { + gravityForce = raylib.NewVector2(0, 9.81/1000) + + bodies = make([]*Body, 0) + + manifolds = make([]*Manifold, 0) + manifoldsCount = 0 +} + +// SetGravity - Sets physics global gravity force +func SetGravity(x, y float32) { + gravityForce.X = x + gravityForce.Y = y +} + +// CreateBodyCircle - Creates a new circle physics body with generic parameters +func CreateBodyCircle(pos raylib.Vector2, radius, density float32) *Body { + bodyCircle := CreateBodyPolygon(pos, radius, circleVertices, density) + //bodyCircle.Shape.Type = Circle + return bodyCircle +} + +// CreateBodyRectangle - Creates a new rectangle physics body with generic parameters +func CreateBodyRectangle(pos raylib.Vector2, width, height, density float32) *Body { + newBody := &Body{} + // Initialize new body with generic values + newBody.Enabled = true + newBody.Position = pos + newBody.Velocity = raylib.Vector2{} + newBody.Force = raylib.Vector2{} + newBody.AngularVelocity = 0 + newBody.Torque = 0 + newBody.Orient = 0 + + newBody.Shape = &Shape{} + newBody.Shape.Type = Polygon + newBody.Shape.Body = newBody + newBody.Shape.VertexData = createRectanglePolygon(pos, raylib.NewVector2(width, height)) + + // Calculate centroid and moment of inertia + center := raylib.Vector2{} + area := float32(0.0) + inertia := float32(0.0) + k := float32(1.0) / 3.0 + + for i := 0; i < newBody.Shape.VertexData.VertexCount; i++ { + // Triangle vertices, third vertex implied as (0, 0) + p1 := newBody.Shape.VertexData.Vertices[i] + nextIndex := 0 + if i+1 < newBody.Shape.VertexData.VertexCount { + nextIndex = i + 1 + } + p2 := newBody.Shape.VertexData.Vertices[nextIndex] + + D := raymath.Vector2CrossProduct(p1, p2) + triangleArea := D / 2 + + area += triangleArea + + // Use area to weight the centroid average, not just vertex position + center.X += triangleArea * k * (p1.X + p2.X) + center.Y += triangleArea * k * (p1.Y + p2.Y) + + intx2 := p1.X*p1.X + p2.X*p1.X + p2.X*p2.X + inty2 := p1.Y*p1.Y + p2.Y*p1.Y + p2.Y*p2.Y + inertia += (0.25 * k * D) * (intx2 + inty2) + } + + center.X *= 1.0 / area + center.Y *= 1.0 / area + + // Translate vertices to centroid (make the centroid (0, 0) for the polygon in model space) + // NOTE: this is not really necessary + for i := 0; i < newBody.Shape.VertexData.VertexCount; i++ { + newBody.Shape.VertexData.Vertices[i].X -= center.X + newBody.Shape.VertexData.Vertices[i].Y -= center.Y + } + + newBody.Mass = density * area + newBody.Inertia = density * inertia + newBody.StaticFriction = 0.4 + newBody.DynamicFriction = 0.2 + newBody.Restitution = 0 + newBody.UseGravity = true + newBody.IsGrounded = false + newBody.FreezeOrient = false + + if newBody.Mass != 0.0 { + newBody.InverseMass = 1.0 / newBody.Mass + } + + if newBody.Inertia != 0.0 { + newBody.InverseInertia = 1.0 / newBody.Inertia + } + + // Add new body to bodies pointers array and update bodies count + bodies = append(bodies, newBody) + bodiesCount++ + + return newBody +} + +// CreateBodyPolygon - Creates a new polygon physics body with generic parameters +func CreateBodyPolygon(pos raylib.Vector2, radius float32, sides int32, density float32) *Body { + newBody := &Body{} + // Initialize new body with generic values + newBody.Enabled = true + newBody.Position = pos + newBody.Velocity = raylib.Vector2{} + newBody.Force = raylib.Vector2{} + newBody.AngularVelocity = 0 + newBody.Torque = 0 + newBody.Orient = 0 + + newBody.Shape = &Shape{} + newBody.Shape.Type = Polygon + newBody.Shape.Body = newBody + newBody.Shape.VertexData = createRandomPolygon(radius, sides) + + // Calculate centroid and moment of inertia + center := raylib.Vector2{} + area := float32(0.0) + inertia := float32(0.0) + alpha := float32(1.0) / 3.0 + + for i := 0; i < newBody.Shape.VertexData.VertexCount; i++ { + // Triangle vertices, third vertex implied as (0, 0) + position1 := newBody.Shape.VertexData.Vertices[i] + nextIndex := 0 + if i+1 < newBody.Shape.VertexData.VertexCount { + nextIndex = i + 1 + } + position2 := newBody.Shape.VertexData.Vertices[nextIndex] + + cross := raymath.Vector2CrossProduct(position1, position2) + triangleArea := cross / 2 + + area += triangleArea + + // Use area to weight the centroid average, not just vertex position + center.X += triangleArea * alpha * (position1.X + position2.X) + center.Y += triangleArea * alpha * (position1.Y + position2.Y) + + intx2 := position1.X*position1.X + position2.X*position1.X + position2.X*position2.X + inty2 := position1.Y*position1.Y + position2.Y*position1.Y + position2.Y*position2.Y + inertia += (0.25 * alpha * cross) * (intx2 + inty2) + } + + center.X *= 1.0 / area + center.Y *= 1.0 / area + + // Translate vertices to centroid (make the centroid (0, 0) for the polygon in model space) + // Note: this is not really necessary + for i := 0; i < newBody.Shape.VertexData.VertexCount; i++ { + newBody.Shape.VertexData.Vertices[i].X -= center.X + newBody.Shape.VertexData.Vertices[i].Y -= center.Y + } + + newBody.Mass = density * area + newBody.Inertia = density * inertia + newBody.StaticFriction = 0.4 + newBody.DynamicFriction = 0.2 + newBody.Restitution = 0 + newBody.UseGravity = true + newBody.IsGrounded = false + newBody.FreezeOrient = false + + if newBody.Mass != 0.0 { + newBody.InverseMass = 1.0 / newBody.Mass + } + + if newBody.Inertia != 0.0 { + newBody.InverseInertia = 1.0 / newBody.Inertia + } + + // Add new body to bodies pointers array and update bodies count + bodies = append(bodies, newBody) + bodiesCount++ + + return newBody +} + +// AddForce - Adds a force to a physics body +func AddForce(body *Body, force raylib.Vector2) { + body.Force = raymath.Vector2Add(body.Force, force) +} + +// AddTorque - Adds an angular force to a physics body +func AddTorque(body *Body, amount float32) { + body.Torque += amount +} + +// Shatter - Shatters a polygon shape physics body to little physics bodies with explosion force +func Shatter(body *Body, position raylib.Vector2, force float32) { +} + +// GetBodiesCount - Returns the current amount of created physics bodies +func GetBodiesCount() int { + return bodiesCount +} + +// GetBody - Returns a physics body of the bodies pool at a specific index +func GetBody(index int) *Body { + body := &Body{} + + if index < bodiesCount { + body = bodies[index] + } else { + raylib.TraceLog(raylib.LogDebug, "[PHYSAC] physics body index is out of bounds") + } + + return body +} + +// GetShapeType - Returns the physics body shape type (Circle or Polygon) +func GetShapeType(index int) ShapeType { + var result ShapeType + + if index < bodiesCount { + result = bodies[index].Shape.Type + } else { + raylib.TraceLog(raylib.LogDebug, "[PHYSAC] physics body index is out of bounds") + } + + return result +} + +// GetShapeVerticesCount - Returns the amount of vertices of a physics body shape +func GetShapeVerticesCount(index int) int { + result := 0 + + if index < bodiesCount { + switch bodies[index].Shape.Type { + case Circle: + result = circleVertices + break + case Polygon: + result = bodies[index].Shape.VertexData.VertexCount + break + } + } else { + raylib.TraceLog(raylib.LogDebug, "[PHYSAC] physics body index is out of bounds") + } + + return result +} + +// GetShapeVertex - Returns transformed position of a body shape (body position + vertex transformed position) +func GetShapeVertex(body *Body, vertex int) raylib.Vector2 { + position := raylib.Vector2{} + + switch body.Shape.Type { + case Circle: + position.X = body.Position.X + float32(math.Cos(360/float64(circleVertices)*float64(vertex)*raylib.Deg2rad))*body.Shape.Radius + position.Y = body.Position.Y + float32(math.Sin(360/float64(circleVertices)*float64(vertex)*raylib.Deg2rad))*body.Shape.Radius + break + case Polygon: + vertexData := body.Shape.VertexData + position = raymath.Vector2Add(body.Position, raymath.Mat2MultiplyVector2(vertexData.Transform, vertexData.Vertices[vertex])) + break + default: + break + } + + return position +} + +// SetBodyRotation - Sets physics body shape transform based on radians parameter +func SetBodyRotation(body *Body, radians float32) { + body.Orient = radians + + if body.Shape.Type == Polygon { + body.Shape.VertexData.Transform = raymath.Mat2Radians(radians) + } +} + +// DestroyBody - Unitializes and destroy a physics body +func DestroyBody(index int) { + // Free body allocated memory + copy(bodies[index:], bodies[index+1:]) + bodies[len(bodies)-1] = &Body{} + bodies = bodies[:len(bodies)-1] + + // Update physics bodies count + bodiesCount-- +} + +// Step - Physics steps calculations (dynamics, collisions and position corrections) +func Step(delta float32) { + deltaTime = delta + + // Clear previous generated collisions information + for i := manifoldsCount - 1; i >= 0; i-- { + destroyManifold(i) + } + + // Reset physics bodies grounded state + for i := 0; i < bodiesCount; i++ { + bodies[i].IsGrounded = false + } + + // Generate new collision information + for i := 0; i < bodiesCount; i++ { + bodyA := bodies[i] + + fmt.Printf("bodyA.Shape %+v\n", bodyA.Shape) + + for j := i + 1; j < bodiesCount; j++ { + bodyB := bodies[j] + + if (bodyA.InverseMass == 0) && (bodyB.InverseMass == 0) { + continue + } + + fmt.Printf("bodyB.Shape %+v\n", bodyB.Shape) + + manifold := &Manifold{} + if bodyA.Shape.Type == Polygon && bodyB.Shape.Type == Circle { + manifold = createManifold(bodyB, bodyA) + } else { + manifold = createManifold(bodyA, bodyB) + } + + solveManifold(manifold) + + if manifold.ContactsCount > 0 { + // Create a new manifold with same information as previously solved manifold and add it to the manifolds pool last slot + newManifold := createManifold(bodyA, bodyB) + newManifold.Penetration = manifold.Penetration + newManifold.Normal = manifold.Normal + newManifold.Contacts[0] = manifold.Contacts[0] + newManifold.Contacts[1] = manifold.Contacts[1] + newManifold.ContactsCount = manifold.ContactsCount + newManifold.Restitution = manifold.Restitution + newManifold.DynamicFriction = manifold.DynamicFriction + newManifold.StaticFriction = manifold.StaticFriction + } + } + } + + // Integrate forces to physics bodies + for i := 0; i < bodiesCount; i++ { + integrateForces(bodies[i]) + } + + // Initialize physics manifolds to solve collisions + for i := 0; i < manifoldsCount; i++ { + initializeManifolds(manifolds[i]) + } + + // Integrate physics collisions impulses to solve collisions + for i := 0; i < collisionIterations; i++ { + for j := 0; j < manifoldsCount; j++ { + integrateImpulses(manifolds[j]) + } + } + + // Integrate velocity to physics bodies + for i := 0; i < bodiesCount; i++ { + integrateVelocity(bodies[i]) + } + + // Correct physics bodies positions based on manifolds collision information + for i := 0; i < manifoldsCount; i++ { + correctPositions(manifolds[i]) + } + + // Clear physics bodies forces + for i := 0; i < bodiesCount; i++ { + bodies[i].Force = raylib.Vector2{} + bodies[i].Torque = 0 + } +} + +// Reset - Destroys created physics bodies and manifolds and resets global values +func Reset() { + bodies = make([]*Body, 0) + bodiesCount = 0 + + manifolds = make([]*Manifold, 0) + manifoldsCount = 0 + + raylib.TraceLog(raylib.LogDebug, "[PHYSAC] physics module reset successfully") +} + +// Close - Unitializes physics pointers +func Close() { + // Unitialize physics manifolds dynamic memory allocations + for i := manifoldsCount - 1; i >= 0; i-- { + destroyManifold(i) + } + + // Unitialize physics bodies dynamic memory allocations + for i := bodiesCount - 1; i >= 0; i-- { + DestroyBody(i) + } +} + +// createRandomPolygon - Creates a random polygon shape with max vertex distance from polygon pivot +func createRandomPolygon(radius float32, sides int32) *PolygonData { + data := &PolygonData{} + data.VertexCount = int(sides) + + orient := raylib.GetRandomValue(0, 360) + data.Transform = raymath.Mat2Radians(float32(orient) * raylib.Deg2rad) + + // Calculate polygon vertices positions + for i := 0; i < data.VertexCount; i++ { + data.Vertices[i].X = float32(math.Cos(360/float64(sides)*float64(i)*raylib.Deg2rad)) * radius + data.Vertices[i].Y = float32(math.Sin(360/float64(sides)*float64(i)*raylib.Deg2rad)) * radius + } + + // Calculate polygon faces normals + for i := 0; i < data.VertexCount; i++ { + nextIndex := 0 + if i+1 < int(sides) { + nextIndex = i + 1 + } + face := raymath.Vector2Subtract(data.Vertices[nextIndex], data.Vertices[i]) + + data.Normals[i] = raylib.NewVector2(face.Y, -face.X) + raymath.Vector2Normalize(&data.Normals[i]) + } + + return data +} + +// createRectanglePolygon - Creates a rectangle polygon shape based on a min and max positions +func createRectanglePolygon(pos, size raylib.Vector2) *PolygonData { + data := &PolygonData{} + + data.VertexCount = 4 + data.Transform = raymath.Mat2Radians(0) + + // Calculate polygon vertices positions + data.Vertices[0] = raylib.NewVector2(pos.X+size.X/2, pos.Y-size.Y/2) + data.Vertices[1] = raylib.NewVector2(pos.X+size.X/2, pos.Y+size.Y/2) + data.Vertices[2] = raylib.NewVector2(pos.X-size.X/2, pos.Y+size.Y/2) + data.Vertices[3] = raylib.NewVector2(pos.X-size.X/2, pos.Y-size.Y/2) + + // Calculate polygon faces normals + for i := 0; i < data.VertexCount; i++ { + nextIndex := 0 + if i+1 < data.VertexCount { + nextIndex = i + 1 + } + face := raymath.Vector2Subtract(data.Vertices[nextIndex], data.Vertices[i]) + + data.Normals[i] = raylib.NewVector2(face.Y, -face.X) + raymath.Vector2Normalize(&data.Normals[i]) + } + + return data +} + +// createManifold - Creates a new physics manifold to solve collision +func createManifold(a, b *Body) *Manifold { + newManifold := &Manifold{} + + // Initialize new manifold with generic values + newManifold.BodyA = a + newManifold.BodyB = b + newManifold.Penetration = 0 + newManifold.Normal = raylib.Vector2{} + newManifold.Contacts[0] = raylib.Vector2{} + newManifold.Contacts[1] = raylib.Vector2{} + newManifold.ContactsCount = 0 + newManifold.Restitution = 0 + newManifold.DynamicFriction = 0 + newManifold.StaticFriction = 0 + + // Add new body to bodies pointers array and update bodies count + manifolds = append(manifolds, newManifold) + manifoldsCount++ + + return newManifold +} + +// destroyManifold - Unitializes and destroys a physics manifold +func destroyManifold(index int) { + // Free manifold allocated memory + copy(manifolds[index:], manifolds[index+1:]) + manifolds[len(manifolds)-1] = &Manifold{} + manifolds = manifolds[:len(manifolds)-1] + + // Update physics manifolds count + manifoldsCount-- +} + +// solveManifold - Solves a created physics manifold between two physics bodies +func solveManifold(manifold *Manifold) { + fmt.Printf("%+v\n\n", manifold) + switch manifold.BodyA.Shape.Type { + case Circle: + switch manifold.BodyB.Shape.Type { + case Circle: + solveCircleToCircle(manifold) + break + case Polygon: + solveCircleToPolygon(manifold) + break + } + case Polygon: + switch manifold.BodyB.Shape.Type { + case Circle: + solvePolygonToCircle(manifold) + break + case Polygon: + solvePolygonToPolygon(manifold) + break + } + } + fmt.Printf("%+v\n\n", manifold) + fmt.Println() + + // Update physics body grounded state if normal direction is down and grounded state is not set yet in previous manifolds + if !manifold.BodyB.IsGrounded { + manifold.BodyB.IsGrounded = (manifold.Normal.Y < 0) + } +} + +// solveCircleToCircle - Solves collision between two circle shape physics bodies +func solveCircleToCircle(manifold *Manifold) { + fmt.Println("solveCircleToCircle") + bodyA := manifold.BodyA + bodyB := manifold.BodyB + + // Calculate translational vector, which is normal + normal := raymath.Vector2Subtract(bodyB.Position, bodyA.Position) + + distSqr := raymath.Vector2LenSqr(normal) + radius := bodyA.Shape.Radius + bodyB.Shape.Radius + + // Check if circles are not in contact + if distSqr >= radius*radius { + manifold.ContactsCount = 0 + return + } + + distance := float32(math.Sqrt(float64(distSqr))) + manifold.ContactsCount = 1 + + if distance == 0 { + manifold.Penetration = bodyA.Shape.Radius + manifold.Normal = raylib.NewVector2(1, 0) + manifold.Contacts[0] = bodyA.Position + } else { + manifold.Penetration = radius - distance + manifold.Normal = raylib.NewVector2(normal.X/distance, normal.Y/distance) // Faster than using MathNormalize() due to sqrt is already performed + manifold.Contacts[0] = raylib.NewVector2(manifold.Normal.X*bodyA.Shape.Radius+bodyA.Position.X, manifold.Normal.Y*bodyA.Shape.Radius+bodyA.Position.Y) + } + + // Update physics body grounded state if normal direction is down + if !bodyA.IsGrounded { + bodyA.IsGrounded = (manifold.Normal.Y < 0) + } +} + +// solveCircleToPolygon - Solves collision between a circle to a polygon shape physics bodies +func solveCircleToPolygon(manifold *Manifold) { + fmt.Println("solveCircleToPolygon") + bodyA := manifold.BodyA + bodyB := manifold.BodyB + + manifold.ContactsCount = 0 + + // Transform circle center to polygon transform space + center := bodyA.Position + center = raymath.Mat2MultiplyVector2(raymath.Mat2Transpose(bodyB.Shape.VertexData.Transform), raymath.Vector2Subtract(center, bodyB.Position)) + + // Find edge with minimum penetration + // It is the same concept as using support points in solvePolygonToPolygon + separation := float32(-fltMax) + faceNormal := 0 + vertexData := bodyB.Shape.VertexData + + for i := 0; i < vertexData.VertexCount; i++ { + currentSeparation := raymath.Vector2DotProduct(vertexData.Normals[i], raymath.Vector2Subtract(center, vertexData.Vertices[i])) + + if currentSeparation > bodyA.Shape.Radius { + return + } + + if currentSeparation > separation { + separation = currentSeparation + faceNormal = i + } + } + + // Grab face's vertices + v1 := vertexData.Vertices[faceNormal] + nextIndex := 0 + if faceNormal+1 < vertexData.VertexCount { + nextIndex = faceNormal + 1 + } + v2 := vertexData.Vertices[nextIndex] + + // Check to see if center is within polygon + if separation < epsilon { + manifold.ContactsCount = 1 + normal := raymath.Mat2MultiplyVector2(vertexData.Transform, vertexData.Normals[faceNormal]) + manifold.Normal = raylib.NewVector2(-normal.X, -normal.Y) + manifold.Contacts[0] = raylib.NewVector2(manifold.Normal.X*bodyA.Shape.Radius+bodyA.Position.X, manifold.Normal.Y*bodyA.Shape.Radius+bodyA.Position.Y) + manifold.Penetration = bodyA.Shape.Radius + return + } + + // Determine which voronoi region of the edge center of circle lies within + dot1 := raymath.Vector2DotProduct(raymath.Vector2Subtract(center, v1), raymath.Vector2Subtract(v2, v1)) + dot2 := raymath.Vector2DotProduct(raymath.Vector2Subtract(center, v2), raymath.Vector2Subtract(v1, v2)) + manifold.Penetration = bodyA.Shape.Radius - separation + + if dot1 <= 0 { // Closest to v1 + if raymath.Vector2Distance(center, v1) > bodyA.Shape.Radius*bodyA.Shape.Radius { + return + } + + manifold.ContactsCount = 1 + normal := raymath.Vector2Subtract(v1, center) + normal = raymath.Mat2MultiplyVector2(vertexData.Transform, normal) + raymath.Vector2Normalize(&normal) + manifold.Normal = normal + v1 = raymath.Mat2MultiplyVector2(vertexData.Transform, v1) + v1 = raymath.Vector2Add(v1, bodyB.Position) + manifold.Contacts[0] = v1 + } else if dot2 <= 0 { // Closest to v2 + if raymath.Vector2Distance(center, v2) > bodyA.Shape.Radius*bodyA.Shape.Radius { + return + } + + manifold.ContactsCount = 1 + normal := raymath.Vector2Subtract(v2, center) + v2 = raymath.Mat2MultiplyVector2(vertexData.Transform, v2) + v2 = raymath.Vector2Add(v2, bodyB.Position) + manifold.Contacts[0] = v2 + normal = raymath.Mat2MultiplyVector2(vertexData.Transform, normal) + raymath.Vector2Normalize(&normal) + manifold.Normal = normal + } else { // Closest to face + normal := vertexData.Normals[faceNormal] + + if raymath.Vector2DotProduct(raymath.Vector2Subtract(center, v1), normal) > bodyA.Shape.Radius { + return + } + + normal = raymath.Mat2MultiplyVector2(vertexData.Transform, normal) + manifold.Normal = raylib.NewVector2(-normal.X, -normal.Y) + manifold.Contacts[0] = raylib.NewVector2(manifold.Normal.X*bodyA.Shape.Radius+bodyA.Position.X, manifold.Normal.Y*bodyA.Shape.Radius+bodyA.Position.Y) + manifold.ContactsCount = 1 + } +} + +// solvePolygonToCircle - Solves collision between a polygon to a circle shape physics bodies +func solvePolygonToCircle(manifold *Manifold) { + fmt.Println("solvePolygonToCircle") + bodyA := manifold.BodyA + bodyB := manifold.BodyB + + manifold.BodyA = bodyB + manifold.BodyB = bodyA + + solveCircleToPolygon(manifold) + + manifold.Normal.X *= -1 + manifold.Normal.Y *= -1 +} + +// solvePolygonToPolygon - Solves collision between two polygons shape physics bodies +func solvePolygonToPolygon(manifold *Manifold) { + //fmt.Println("solvePolygonToPolygon") + shapeA := manifold.BodyA.Shape + shapeB := manifold.BodyB.Shape + + manifold.ContactsCount = 0 + + // Check for separating axis with A shape's face planes + faceA := 0 + penetrationA := findAxisLeastPenetration(&faceA, shapeA, shapeB) + if penetrationA >= 0 { + return + } + + // Check for separating axis with B shape's face planes + faceB := 0 + penetrationB := findAxisLeastPenetration(&faceB, shapeB, shapeA) + if penetrationB >= 0 { + return + } + + referenceIndex := 0 + flip := false // Always point from A shape to B shape + + refPoly := &Shape{} // Reference + incPoly := &Shape{} // Incident + + // Determine which shape contains reference face + if biasGreaterThan(penetrationA, penetrationB) { + refPoly = shapeA + incPoly = shapeB + referenceIndex = faceA + } else { + refPoly = shapeB + incPoly = shapeA + referenceIndex = faceB + flip = true + } + + // World space incident face + //incidentFace := make([]raylib.Vector2, 2) + incidentFace0 := raylib.Vector2{} + incidentFace1 := raylib.Vector2{} + findIncidentFace(&incidentFace0, &incidentFace1, refPoly, incPoly, referenceIndex) + + // Setup reference face vertices + refData := refPoly.VertexData + v1 := refData.Vertices[referenceIndex] + if referenceIndex+1 < refData.VertexCount { + referenceIndex = referenceIndex + 1 + } + v2 := refData.Vertices[referenceIndex] + + // Transform vertices to world space + v1 = raymath.Mat2MultiplyVector2(refData.Transform, v1) + v1 = raymath.Vector2Add(v1, refPoly.Body.Position) + v2 = raymath.Mat2MultiplyVector2(refData.Transform, v2) + v2 = raymath.Vector2Add(v2, refPoly.Body.Position) + + // Calculate reference face side normal in world space + sidePlaneNormal := raymath.Vector2Subtract(v2, v1) + raymath.Vector2Normalize(&sidePlaneNormal) + + // Orthogonalize + refFaceNormal := raylib.NewVector2(sidePlaneNormal.Y, -sidePlaneNormal.X) + refC := raymath.Vector2DotProduct(refFaceNormal, v1) + negSide := raymath.Vector2DotProduct(sidePlaneNormal, v1) * -1 + posSide := raymath.Vector2DotProduct(sidePlaneNormal, v2) + + // clip incident face to reference face side planes (due to floating point error, possible to not have required points + if clip(raylib.NewVector2(-sidePlaneNormal.X, -sidePlaneNormal.Y), negSide, &incidentFace0, &incidentFace1) < 2 { + return + } + if clip(sidePlaneNormal, posSide, &incidentFace0, &incidentFace1) < 2 { + return + } + + // Flip normal if required + if flip { + manifold.Normal = raylib.NewVector2(-refFaceNormal.X, -refFaceNormal.Y) + } else { + manifold.Normal = refFaceNormal + } + + // Keep points behind reference face + currentPoint := 0 // clipped points behind reference face + separation := raymath.Vector2DotProduct(refFaceNormal, incidentFace0) - refC + if separation <= 0 { + manifold.Contacts[currentPoint] = incidentFace0 + manifold.Penetration = -separation + currentPoint++ + } else { + manifold.Penetration = 0 + } + + separation = raymath.Vector2DotProduct(refFaceNormal, incidentFace1) - refC + + if separation <= 0 { + manifold.Contacts[currentPoint] = incidentFace1 + manifold.Penetration += -separation + currentPoint++ + + // Calculate total penetration average + manifold.Penetration /= float32(currentPoint) + } + + manifold.ContactsCount = currentPoint +} + +// integrateForces - Integrates physics forces into velocity +func integrateForces(body *Body) { + if body.InverseMass == 0 || !body.Enabled { + return + } + + body.Velocity.X += (body.Force.X * body.InverseMass) * (deltaTime / 2) + body.Velocity.Y += (body.Force.Y * body.InverseMass) * (deltaTime / 2) + + if body.UseGravity { + body.Velocity.X += gravityForce.X * (deltaTime / 2) + body.Velocity.Y += gravityForce.Y * (deltaTime / 2) + } + + if !body.FreezeOrient { + body.AngularVelocity += body.Torque * body.InverseInertia * (deltaTime / 2) + } +} + +// initializeManifolds - Initializes physics manifolds to solve collisions +func initializeManifolds(manifold *Manifold) { + bodyA := manifold.BodyA + bodyB := manifold.BodyB + + // Calculate average restitution, static and dynamic friction + manifold.Restitution = float32(math.Sqrt(float64(bodyA.Restitution * bodyB.Restitution))) + manifold.StaticFriction = float32(math.Sqrt(float64(bodyA.StaticFriction * bodyB.StaticFriction))) + manifold.DynamicFriction = float32(math.Sqrt(float64(bodyA.DynamicFriction * bodyB.DynamicFriction))) + + for i := 0; i < 2; i++ { + // Caculate radius from center of mass to contact + radiusA := raymath.Vector2Subtract(manifold.Contacts[i], bodyA.Position) + radiusB := raymath.Vector2Subtract(manifold.Contacts[i], bodyB.Position) + + crossA := raymath.Vector2Cross(bodyA.AngularVelocity, radiusA) + crossB := raymath.Vector2Cross(bodyB.AngularVelocity, radiusB) + + radiusV := raylib.Vector2{} + radiusV.X = bodyB.Velocity.X + crossB.X - bodyA.Velocity.X - crossA.X + radiusV.Y = bodyB.Velocity.Y + crossB.Y - bodyA.Velocity.Y - crossA.Y + + // Determine if we should perform a resting collision or not; + // The idea is if the only thing moving this object is gravity, then the collision should be performed without any restitution + if raymath.Vector2LenSqr(radiusV) < (raymath.Vector2LenSqr(raylib.NewVector2(gravityForce.X*deltaTime, gravityForce.Y*deltaTime)) + epsilon) { + manifold.Restitution = 0 + } + } +} + +// integrateImpulses - Integrates physics collisions impulses to solve collisions +func integrateImpulses(manifold *Manifold) { + bodyA := manifold.BodyA + bodyB := manifold.BodyB + + // Early out and positional correct if both objects have infinite mass + if float32(math.Abs(float64(bodyA.InverseMass+bodyB.InverseMass))) <= epsilon { + bodyA.Velocity = raylib.Vector2{} + bodyB.Velocity = raylib.Vector2{} + return + } + + for i := 0; i < manifold.ContactsCount; i++ { + // Calculate radius from center of mass to contact + radiusA := raymath.Vector2Subtract(manifold.Contacts[i], bodyA.Position) + radiusB := raymath.Vector2Subtract(manifold.Contacts[i], bodyB.Position) + + // Calculate relative velocity + radiusV := raylib.Vector2{} + radiusV.X = bodyB.Velocity.X + raymath.Vector2Cross(bodyB.AngularVelocity, radiusB).X - bodyA.Velocity.X - raymath.Vector2Cross(bodyA.AngularVelocity, radiusA).X + radiusV.Y = bodyB.Velocity.Y + raymath.Vector2Cross(bodyB.AngularVelocity, radiusB).Y - bodyA.Velocity.Y - raymath.Vector2Cross(bodyA.AngularVelocity, radiusA).Y + + // Relative velocity along the normal + contactVelocity := raymath.Vector2DotProduct(radiusV, manifold.Normal) + + // Do not resolve if velocities are separating + if contactVelocity > 0 { + return + } + + raCrossN := raymath.Vector2CrossProduct(radiusA, manifold.Normal) + rbCrossN := raymath.Vector2CrossProduct(radiusB, manifold.Normal) + + inverseMassSum := bodyA.InverseMass + bodyB.InverseMass + (raCrossN*raCrossN)*bodyA.InverseInertia + (rbCrossN*rbCrossN)*bodyB.InverseInertia + + // Calculate impulse scalar value + impulse := -(1.0 + manifold.Restitution) * contactVelocity + impulse /= inverseMassSum + impulse /= float32(manifold.ContactsCount) + + // Apply impulse to each physics body + impulseV := raylib.NewVector2(manifold.Normal.X*impulse, manifold.Normal.Y*impulse) + + if bodyA.Enabled { + bodyA.Velocity.X += bodyA.InverseMass * (-impulseV.X) + bodyA.Velocity.Y += bodyA.InverseMass * (-impulseV.Y) + if !bodyA.FreezeOrient { + bodyA.AngularVelocity += bodyA.InverseInertia * raymath.Vector2CrossProduct(radiusA, raylib.NewVector2(-impulseV.X, -impulseV.Y)) + } + } + + if bodyB.Enabled { + bodyB.Velocity.X += bodyB.InverseMass * (impulseV.X) + bodyB.Velocity.Y += bodyB.InverseMass * (impulseV.Y) + if !bodyB.FreezeOrient { + bodyB.AngularVelocity += bodyB.InverseInertia * raymath.Vector2CrossProduct(radiusB, impulseV) + } + } + + // Apply friction impulse to each physics body + radiusV.X = bodyB.Velocity.X + raymath.Vector2Cross(bodyB.AngularVelocity, radiusB).X - bodyA.Velocity.X - raymath.Vector2Cross(bodyA.AngularVelocity, radiusA).X + radiusV.Y = bodyB.Velocity.Y + raymath.Vector2Cross(bodyB.AngularVelocity, radiusB).Y - bodyA.Velocity.Y - raymath.Vector2Cross(bodyA.AngularVelocity, radiusA).Y + + tangent := raylib.NewVector2(radiusV.X-(manifold.Normal.X*raymath.Vector2DotProduct(radiusV, manifold.Normal)), radiusV.Y-(manifold.Normal.Y*raymath.Vector2DotProduct(radiusV, manifold.Normal))) + raymath.Vector2Normalize(&tangent) + + // Calculate impulse tangent magnitude + impulseTangent := -raymath.Vector2DotProduct(radiusV, tangent) + impulseTangent /= inverseMassSum + impulseTangent /= float32(manifold.ContactsCount) + + absImpulseTangent := float32(math.Abs(float64(impulseTangent))) + + // Don't apply tiny friction impulses + if absImpulseTangent <= epsilon { + return + } + + // Apply coulumb's law + tangentImpulse := raylib.Vector2{} + if absImpulseTangent < impulse*manifold.StaticFriction { + tangentImpulse = raylib.NewVector2(tangent.X*impulseTangent, tangent.Y*impulseTangent) + } else { + tangentImpulse = raylib.NewVector2(tangent.X*-impulse*manifold.DynamicFriction, tangent.Y*-impulse*manifold.DynamicFriction) + } + + // Apply friction impulse + if bodyA.Enabled { + bodyA.Velocity.X += bodyA.InverseMass * (-tangentImpulse.X) + bodyA.Velocity.Y += bodyA.InverseMass * (-tangentImpulse.Y) + + if !bodyA.FreezeOrient { + bodyA.AngularVelocity += bodyA.InverseInertia * raymath.Vector2CrossProduct(radiusA, raylib.NewVector2(-tangentImpulse.X, -tangentImpulse.Y)) + } + } + + if bodyB.Enabled { + bodyB.Velocity.X += bodyB.InverseMass * (tangentImpulse.X) + bodyB.Velocity.Y += bodyB.InverseMass * (tangentImpulse.Y) + + if !bodyB.FreezeOrient { + bodyB.AngularVelocity += bodyB.InverseInertia * raymath.Vector2CrossProduct(radiusB, tangentImpulse) + } + } + } +} + +// integrateVelocity - Integrates physics velocity into position and forces +func integrateVelocity(body *Body) { + if !body.Enabled { + return + } + + body.Position.X += body.Velocity.X * deltaTime + body.Position.Y += body.Velocity.Y * deltaTime + + if !body.FreezeOrient { + body.Orient += body.AngularVelocity * deltaTime + } + + raymath.Mat2Set(&body.Shape.VertexData.Transform, body.Orient) + + integrateForces(body) +} + +// correctPositions - Corrects physics bodies positions based on manifolds collision information +func correctPositions(manifold *Manifold) { + bodyA := manifold.BodyA + bodyB := manifold.BodyB + + correction := raylib.Vector2{} + correction.X = float32(math.Max(float64(manifold.Penetration-penetrationAllowance), 0)) / (bodyA.InverseMass + bodyB.InverseMass) * manifold.Normal.X * penetrationCorrection + correction.Y = float32(math.Max(float64(manifold.Penetration-penetrationAllowance), 0)) / (bodyA.InverseMass + bodyB.InverseMass) * manifold.Normal.Y * penetrationCorrection + + if bodyA.Enabled { + bodyA.Position.X -= correction.X * bodyA.InverseMass + bodyA.Position.Y -= correction.Y * bodyA.InverseMass + } + + if bodyB.Enabled { + bodyB.Position.X += correction.X * bodyB.InverseMass + bodyB.Position.Y += correction.Y * bodyB.InverseMass + } +} + +// Returns the extreme point along a direction within a polygon +func getSupport(shape *Shape, dir raylib.Vector2) raylib.Vector2 { + bestProjection := float32(-fltMax) + bestVertex := raylib.Vector2{} + data := shape.VertexData + + for i := 0; i < data.VertexCount; i++ { + vertex := data.Vertices[i] + projection := raymath.Vector2DotProduct(vertex, dir) + + if projection > bestProjection { + bestVertex = vertex + bestProjection = projection + } + } + + return bestVertex +} + +// findAxisLeastPenetration - Finds polygon shapes axis least penetration +func findAxisLeastPenetration(faceIndex *int, shapeA, shapeB *Shape) float32 { + bestDistance := float32(-fltMax) + bestIndex := 0 + + dataA := shapeA.VertexData + dataB := shapeB.VertexData + + for i := 0; i < dataA.VertexCount; i++ { + // Retrieve a face normal from A shape + normal := dataA.Normals[i] + transNormal := raymath.Mat2MultiplyVector2(dataA.Transform, normal) + + // Transform face normal into B shape's model space + buT := raymath.Mat2Transpose(dataB.Transform) + normal = raymath.Mat2MultiplyVector2(buT, transNormal) + + // Retrieve support point from B shape along -n + support := getSupport(shapeB, raylib.NewVector2(-normal.X, -normal.Y)) + + // Retrieve vertex on face from A shape, transform into B shape's model space + vertex := dataA.Vertices[i] + vertex = raymath.Mat2MultiplyVector2(dataA.Transform, vertex) + vertex = raymath.Vector2Add(vertex, shapeA.Body.Position) + vertex = raymath.Vector2Subtract(vertex, shapeB.Body.Position) + vertex = raymath.Mat2MultiplyVector2(buT, vertex) + + // Compute penetration distance in B shape's model space + distance := raymath.Vector2DotProduct(normal, raymath.Vector2Subtract(support, vertex)) + + // Store greatest distance + if distance > bestDistance { + bestDistance = distance + bestIndex = i + } + } + + *faceIndex = bestIndex + + return bestDistance +} + +// findIncidentFace - Finds two polygon shapes incident face +func findIncidentFace(v0, v1 *raylib.Vector2, ref, inc *Shape, index int) { + refData := ref.VertexData + incData := inc.VertexData + + referenceNormal := refData.Normals[index] + + // Calculate normal in incident's frame of reference + referenceNormal = raymath.Mat2MultiplyVector2(refData.Transform, referenceNormal) // To world space + referenceNormal = raymath.Mat2MultiplyVector2(raymath.Mat2Transpose(incData.Transform), referenceNormal) // To incident's model space + + // Find most anti-normal face on polygon + incidentFace := 0 + minDot := float32(fltMax) + + for i := 0; i < incData.VertexCount; i++ { + dot := raymath.Vector2DotProduct(referenceNormal, incData.Normals[i]) + + if dot < minDot { + minDot = dot + incidentFace = i + } + } + + // Assign face vertices for incident face + *v0 = raymath.Mat2MultiplyVector2(incData.Transform, incData.Vertices[incidentFace]) + *v0 = raymath.Vector2Add(*v0, inc.Body.Position) + + if incidentFace+1 < incData.VertexCount { + incidentFace = incidentFace + 1 + } + + *v1 = raymath.Mat2MultiplyVector2(incData.Transform, incData.Vertices[incidentFace]) + *v1 = raymath.Vector2Add(*v1, inc.Body.Position) +} + +// clip - Calculates clipping based on a normal and two faces +func clip(normal raylib.Vector2, clip float32, faceA, faceB *raylib.Vector2) int { + sp := 0 + + out := make([]raylib.Vector2, 2) + out[0] = *faceA + out[1] = *faceB + + // Retrieve distances from each endpoint to the line + distanceA := raymath.Vector2DotProduct(normal, *faceA) - clip + distanceB := raymath.Vector2DotProduct(normal, *faceB) - clip + + // If negative (behind plane) + if distanceA <= 0 { + spp := sp + 1 + out[spp] = *faceA + } + if distanceB <= 0 { + spp := sp + 1 + out[spp] = *faceB + } + + // If the points are on different sides of the plane + if distanceA*distanceB < 0 { + // Push intersection point + alpha := distanceA / (distanceA - distanceB) + out[sp] = *faceA + delta := raymath.Vector2Subtract(*faceB, *faceA) + delta.X *= alpha + delta.Y *= alpha + out[sp] = raymath.Vector2Add(out[sp], delta) + sp++ + } + + // Assign the new converted values + *faceA = out[0] + *faceB = out[1] + + return sp +} + +// biasGreaterThan - Check if values are between bias range +func biasGreaterThan(valueA, valueB float32) bool { + return valueA >= (valueB*0.95 + valueA*0.01) +} + +// triangleBarycenter - Returns the barycenter of a triangle given by 3 points +func triangleBarycenter(v1, v2, v3 raylib.Vector2) raylib.Vector2 { + result := raylib.Vector2{} + + result.X = (v1.X + v2.X + v3.X) / 3 + result.Y = (v1.Y + v2.Y + v3.Y) / 3 + + return result }