// Package physics - 2D Physics library for videogames // // A port of Victor Fisac's physac engine (https://github.com/raysan5/raylib/blob/master/src/physac.h) package physics import ( "fmt" "math" "github.com/gen2brain/raylib-go/raylib" "github.com/gen2brain/raylib-go/raymath" ) // Physics shape type type ShapeType int // 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 int // Polygon vertex positions vectors Vertices [maxVertices]raylib.Vector2 // Polygon vertex normals vectors Normals [maxVertices]raylib.Vector2 // Vertices transform matrix 2x2 Transform raylib.Mat2 } // Shape type type Shape struct { // Physics shape type (circle or polygon) Type ShapeType // Shape physics body reference 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 } // Body type type Body struct { // Enabled dynamics state (collisions are calculated anyway) Enabled bool // Physics body shape pivot Position raylib.Vector2 // Current linear velocity applied to position Velocity raylib.Vector2 // Current linear force (reset to 0 every step) Force raylib.Vector2 // Current angular velocity applied to orient AngularVelocity float32 // Current angular force (reset to 0 every step) Torque float32 // Rotation in radians Orient float32 // Moment of inertia Inertia float32 // Inverse value of inertia InverseInertia float32 // Physics body mass Mass float32 // Inverse value of mass InverseMass float32 // Friction when the body has not movement (0 to 1) StaticFriction float32 // Friction when the body has movement (0 to 1) DynamicFriction float32 // Restitution coefficient of the body (0 to 1) Restitution float32 // Apply gravity force to dynamics UseGravity bool // Physics grounded on other body state IsGrounded bool // Physics rotation constraint FreezeOrient bool // Physics body shape information (type, radius, vertices, normals) Shape *Shape } // Manifold type type Manifold struct { // Manifold first physics body reference BodyA *Body // Manifold second physics body reference BodyB *Body // Depth of penetration from collision Penetration float32 // Normal direction vector from 'a' to 'b' Normal raylib.Vector2 // Points of contact during collision Contacts [2]raylib.Vector2 // Current collision number of contacts ContactsCount int // Mixed restitution during collision Restitution float32 // Mixed dynamic friction during collision DynamicFriction float32 // Mixed static friction during collision StaticFriction float32 } // 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 }