diff --git a/examples/physics/physac/demo/main.go b/examples/physics/physac/demo/main.go index 8fb3777..c4ff126 100644 --- a/examples/physics/physac/demo/main.go +++ b/examples/physics/physac/demo/main.go @@ -2,7 +2,7 @@ package main import ( "github.com/gen2brain/raylib-go/physics" - "github.com/gen2brain/raylib-go/raylib" + rl "github.com/gen2brain/raylib-go/raylib" ) func main() { @@ -52,9 +52,10 @@ func main() { } // Destroy falling physics bodies - for _, body := range physics.GetBodies() { + for i := 0; i < physics.GetBodiesCount(); i++ { + body := physics.GetBody(i) if body.Position.Y > float32(screenHeight)*2 { - physics.DestroyBody(body) + body.Destroy() } } @@ -65,7 +66,9 @@ func main() { rl.DrawFPS(screenWidth-90, screenHeight-30) // Draw created physics bodies - for i, body := range physics.GetBodies() { + for i := 0; i < physics.GetBodiesCount(); i++ { + body := physics.GetBody(i) + vertexCount := physics.GetShapeVerticesCount(i) for j := 0; j < vertexCount; j++ { // Get physics bodies shape vertices to draw lines diff --git a/examples/physics/physac/shatter/main.go b/examples/physics/physac/shatter/main.go index 56ad42f..f2ae1cd 100644 --- a/examples/physics/physac/shatter/main.go +++ b/examples/physics/physac/shatter/main.go @@ -2,7 +2,7 @@ package main import ( "github.com/gen2brain/raylib-go/physics" - "github.com/gen2brain/raylib-go/raylib" + rl "github.com/gen2brain/raylib-go/raylib" ) const ( @@ -41,8 +41,14 @@ func main() { } if rl.IsMouseButtonPressed(rl.MouseLeftButton) { - for _, b := range physics.GetBodies() { - b.Shatter(rl.GetMousePosition(), 10/b.InverseMass) + //for _, b := range physics.GetBodies() { + for i := 0; i < physics.GetBodiesCount(); i++ { + body := physics.GetBody(i) + if body == nil { + continue + } + + physics.Shatter(body, rl.GetMousePosition(), 10/body.InverseMass) } } diff --git a/physics/physics.go b/physics/physics.go index fe2d33f..91cf03a 100644 --- a/physics/physics.go +++ b/physics/physics.go @@ -5,8 +5,10 @@ package physics import ( "math" + "math/rand" + "time" - "github.com/gen2brain/raylib-go/raylib" + rl "github.com/gen2brain/raylib-go/raylib" ) // ShapeType type @@ -25,11 +27,9 @@ type Polygon struct { // Current used vertex and normals count VertexCount int // Polygon vertex positions vectors - Vertices [maxVertices]rl.Vector2 + Positions [maxVertices]rl.Vector2 // Polygon vertex normals vectors Normals [maxVertices]rl.Vector2 - // Vertices transform matrix 2x2 - Transform rl.Mat2 } // Shape type @@ -40,12 +40,16 @@ type Shape struct { Body *Body // Circle shape radius (used for circle shapes) Radius float32 + // Vertices transform matrix 2x2 + Transform rl.Mat2 // Polygon shape vertices position and normals data (just used for polygon shapes) VertexData Polygon } // Body type type Body struct { + // Reference unique identifier + ID int // Enabled dynamics state (collisions are calculated anyway) Enabled bool // Physics body shape pivot @@ -84,8 +88,10 @@ type Body struct { Shape Shape } -// manifold type -type manifold struct { +// Manifold type +type Manifold struct { + // Reference unique identifier + ID int // Manifold first physics body reference BodyA *Body // Manifold second physics body reference @@ -108,9 +114,8 @@ type manifold struct { // Constants const ( - maxBodies = 64 - maxManifolds = 4096 - + maxBodies = 64 + maxManifolds = 4096 maxVertices = 24 circleVertices = 24 @@ -118,36 +123,51 @@ const ( penetrationAllowance = 0.05 penetrationCorrection = 0.4 - epsilon = 0.000001 + degToRad = math.Pi / 180 + epsilon = 0.000001 + physacK = 1.0 / 3.0 ) // Globals var ( - // Physics bodies pointers - bodies []*Body + // Offset time for MONOTONIC clock + baseTime = time.Now() - // Physics manifolds pointers - manifolds []*manifold - - // Physics world gravity force - gravityForce rl.Vector2 + // Start time in milliseconds + startTime float32 // Delta time used for physics steps, in milliseconds - deltaTime float32 -) + deltaTime float32 = 1.0 / 60.0 / 10.0 * 1000 -// Init - initializes physics values -func Init() { - deltaTime = 1.0 / 60.0 / 10.0 * 1000 + // Current time in milliseconds + currentTime float32 + + // Hi-res clock frequency + frequency uint64 = 0 + + // Physics time step delta time accumulator + accumulator float32 + + // Physics world gravity force gravityForce = rl.NewVector2(0, 9.81) - bodies = make([]*Body, 0, maxBodies) - manifolds = make([]*manifold, 0, maxManifolds) -} + // Physics bodies pointers array + bodies [64]*Body -// Sets physics fixed time step in milliseconds. 1.666666 by default -func SetPhysicsTimeStep(delta float32) { - deltaTime = delta + // Physics world current bodies counter + bodiesCount int + + // Physics bodies pointers array + manifolds [4096]*Manifold + + // Physics world current manifolds counter + manifoldsCount int +) + +// Init - Initializes physics values, pointers and creates physics loop thread +func Init() { + initTimer() + accumulator = 0 } // SetGravity - Sets physics global gravity force @@ -158,41 +178,89 @@ func SetGravity(x, y float32) { // NewBodyCircle - Creates a new circle physics body with generic parameters func NewBodyCircle(pos rl.Vector2, radius, density float32) *Body { - return NewBodyPolygon(pos, radius, circleVertices, density) + newID := findAvailableBodyIndex() + if newID < 0 { + return nil + } + + // Initialize new body with generic values + newBody := &Body{ + ID: newID, + Enabled: true, + Position: pos, + Velocity: rl.Vector2{}, + Force: rl.Vector2{}, + AngularVelocity: 0.0, + Torque: 0.0, + Orient: 0.0, + Shape: Shape{ + Type: CircleShape, + Radius: radius, + Transform: rl.Mat2Radians(0.0), + VertexData: Polygon{}, + }, + StaticFriction: 0.4, + DynamicFriction: 0.2, + Restitution: 0.0, + UseGravity: true, + IsGrounded: false, + FreezeOrient: false, + } + + newBody.Shape.Body = newBody + + newBody.Mass = math.Pi * radius * radius * density + newBody.InverseMass = safeDiv(1.0, newBody.Mass) + newBody.Inertia = newBody.Mass * radius * radius + newBody.InverseInertia = safeDiv(1.0, newBody.Inertia) + + // Add new body to bodies pointers array and update bodies count + bodies[bodiesCount] = newBody + bodiesCount++ + return newBody } // NewBodyRectangle - Creates a new rectangle physics body with generic parameters func NewBodyRectangle(pos rl.Vector2, width, height, density float32) *Body { - newBody := &Body{} + newID := findAvailableBodyIndex() + if newID < 0 { + return nil + } // Initialize new body with generic values - newBody.Enabled = true - newBody.Position = pos - newBody.Velocity = rl.Vector2{} - newBody.Force = rl.Vector2{} - newBody.AngularVelocity = 0 - newBody.Torque = 0 - newBody.Orient = 0 - - newBody.Shape = Shape{} - newBody.Shape.Type = PolygonShape - newBody.Shape.Body = newBody - newBody.Shape.VertexData = newRectanglePolygon(pos, rl.NewVector2(width, height)) + newBody := &Body{ + ID: newID, + Enabled: true, + Position: pos, + Velocity: rl.Vector2{}, + Force: rl.Vector2{}, + AngularVelocity: 0.0, + Torque: 0.0, + Orient: 0.0, + Shape: Shape{ + Type: PolygonShape, + Transform: rl.Mat2Radians(0.0), + VertexData: createRectanglePolygon(pos, rl.NewVector2(width, height)), + }, + StaticFriction: 0.4, + DynamicFriction: 0.2, + Restitution: 0.0, + UseGravity: true, + IsGrounded: false, + FreezeOrient: false, + } // Calculate centroid and moment of inertia - center := rl.Vector2{} + newBody.Shape.Body = newBody + var center rl.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] + nextIndex := getNextIndex(i, newBody.Shape.VertexData.VertexCount) + p1 := newBody.Shape.VertexData.Positions[i] + p2 := newBody.Shape.VertexData.Positions[nextIndex] D := rl.Vector2CrossProduct(p1, p2) triangleArea := D / 2 @@ -200,92 +268,12 @@ func NewBodyRectangle(pos rl.Vector2, width, height, density float32) *Body { 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) + center.X += triangleArea * physacK * (p1.X + p2.X) + center.Y += triangleArea * physacK * (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 { - newBody.InverseMass = 1.0 / newBody.Mass - } - - if newBody.Inertia != 0 { - newBody.InverseInertia = 1.0 / newBody.Inertia - } - - // Add new body to bodies pointers - bodies = append(bodies, newBody) - - return newBody -} - -// NewBodyPolygon - Creates a new polygon physics body with generic parameters -func NewBodyPolygon(pos rl.Vector2, radius float32, sides int, density float32) *Body { - newBody := &Body{} - - // Initialize new body with generic values - newBody.Enabled = true - newBody.Position = pos - newBody.Velocity = rl.Vector2{} - newBody.Force = rl.Vector2{} - newBody.AngularVelocity = 0 - newBody.Torque = 0 - newBody.Orient = 0 - - newBody.Shape = Shape{} - newBody.Shape.Type = PolygonShape - newBody.Shape.Body = newBody - newBody.Shape.VertexData = newRandomPolygon(radius, sides) - - // Calculate centroid and moment of inertia - center := rl.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 := rl.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) + var intx2 float32 = p1.X*p1.X + p2.X*p1.X + p2.X*p2.X + var inty2 float32 = p1.Y*p1.Y + p2.Y*p1.Y + p2.Y*p2.Y + inertia += (0.25 * physacK * D) * (intx2 + inty2) } center.X *= 1.0 / area @@ -294,247 +282,140 @@ func NewBodyPolygon(pos rl.Vector2, radius float32, sides int, density float32) // 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.Shape.VertexData.Positions[i].X -= center.X + newBody.Shape.VertexData.Positions[i].Y -= center.Y } newBody.Mass = density * area + newBody.InverseMass = safeDiv(1.0, newBody.Mass) 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 { - newBody.InverseMass = 1.0 / newBody.Mass - } - - if newBody.Inertia != 0 { - newBody.InverseInertia = 1.0 / newBody.Inertia - } - - // Add new body to bodies pointers - bodies = append(bodies, newBody) + newBody.InverseInertia = safeDiv(1.0, newBody.Inertia) + // Add new body to bodies pointers array and update bodies count + bodies[bodiesCount] = newBody + bodiesCount++ return newBody } -// GetBodies - Returns the slice of created physics bodies -func GetBodies() []*Body { - return bodies -} - -// GetBodiesCount - Returns the current amount of created physics bodies -func GetBodiesCount() int { - return len(bodies) -} - -// GetBody - Returns a physics body of the bodies pool at a specific index -func GetBody(index int) *Body { - var body *Body - - if index < len(bodies) { - body = bodies[index] - } else { - rl.TraceLog(rl.LogDebug, "[PHYSAC] physics body index is out of bounds") +// NewBodyPolygon - Creates a new polygon physics body with generic parameters +func NewBodyPolygon(pos rl.Vector2, radius float32, sides int, density float32) *Body { + newID := findAvailableBodyIndex() + if newID < 0 { + return nil } - return body -} - -// GetShapeType - Returns the physics body shape type (Circle or Polygon) -func GetShapeType(index int) ShapeType { - var result ShapeType - - if index < len(bodies) { - result = bodies[index].Shape.Type - } else { - rl.TraceLog(rl.LogDebug, "[PHYSAC] physics body index is out of bounds") + // Initialize new body with generic values + newBody := &Body{ + ID: newID, + Enabled: true, + Position: pos, + Velocity: rl.Vector2{}, + Force: rl.Vector2{}, + AngularVelocity: 0.0, + Torque: 0.0, + Orient: 0.0, + Shape: Shape{ + Type: PolygonShape, + Transform: rl.Mat2Radians(0), + VertexData: createRandomPolygon(radius, sides), + }, + StaticFriction: 0.4, + DynamicFriction: 0.2, + Restitution: 0.0, + UseGravity: true, + IsGrounded: false, + FreezeOrient: false, } - return result -} + newBody.Shape.Body = newBody -// GetShapeVerticesCount - Returns the amount of vertices of a physics body shape -func GetShapeVerticesCount(index int) int { - result := 0 + // Calculate centroid and moment of inertia + var center rl.Vector2 + area := float32(0.0) + inertia := float32(0.0) - if index < len(bodies) { - switch bodies[index].Shape.Type { - case CircleShape: - result = circleVertices - break - case PolygonShape: - result = bodies[index].Shape.VertexData.VertexCount - break - } - } else { - rl.TraceLog(rl.LogDebug, "[PHYSAC] physics body index is out of bounds") + for i := 0; i < newBody.Shape.VertexData.VertexCount; i++ { + // Triangle vertices, third vertex implied as (0, 0) + nextIndex := getNextIndex(i, newBody.Shape.VertexData.VertexCount) + position1 := newBody.Shape.VertexData.Positions[i] + position2 := newBody.Shape.VertexData.Positions[nextIndex] + + cross := rl.Vector2CrossProduct(position1, position2) + triangleArea := cross / 2 + + area += triangleArea + + // Use area to weight the centroid average, not just vertex position + center.X += triangleArea * physacK * (position1.X + position2.X) + center.Y += triangleArea * physacK * (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 * physacK * cross) * (intx2 + inty2) } - return result -} + center.X *= 1.0 / area + center.Y *= 1.0 / area -// DestroyBody - Unitializes and destroys a physics body -func DestroyBody(body *Body) bool { - for index, b := range bodies { - if b == body { - // Free body allocated memory - bodies = append(bodies[:index], bodies[index+1:]...) - return true - } + // 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.Positions[i].X -= center.X + newBody.Shape.VertexData.Positions[i].Y -= center.Y } - return false -} + newBody.Mass = density * area + newBody.InverseMass = safeDiv(1.0, newBody.Mass) + newBody.Inertia = density * inertia + newBody.InverseInertia = safeDiv(1.0, newBody.Inertia) -// Update - Physics steps calculations (dynamics, collisions and position corrections) -func Update() { - deltaTime = rl.GetFrameTime() * 1000 - - // Clear previous generated collisions information - for _, m := range manifolds { - destroyManifold(m) - } - - // Reset physics bodies grounded state - for _, b := range bodies { - b.IsGrounded = false - } - - // Generate new collision information - bodiesCount := len(bodies) - for i := 0; i < bodiesCount; i++ { - - bodyA := bodies[i] - - for j := i + 1; j < bodiesCount; j++ { - - bodyB := bodies[j] - - if (bodyA.InverseMass == 0) && (bodyB.InverseMass == 0) { - continue - } - - var m *manifold - if bodyA.Shape.Type == PolygonShape && bodyB.Shape.Type == CircleShape { - m = newManifold(bodyB, bodyA) - } else { - m = newManifold(bodyA, bodyB) - } - - m.solveManifold() - - if m.ContactsCount > 0 { - // Create a new manifold with same information as previously solved manifold and add it to the manifolds pool last slot - newManifold := newManifold(bodyA, bodyB) - newManifold.Penetration = m.Penetration - newManifold.Normal = m.Normal - newManifold.Contacts[0] = m.Contacts[0] - newManifold.Contacts[1] = m.Contacts[1] - newManifold.ContactsCount = m.ContactsCount - newManifold.Restitution = m.Restitution - newManifold.DynamicFriction = m.DynamicFriction - newManifold.StaticFriction = m.StaticFriction - } - } - } - - // Integrate forces to physics bodies - for _, b := range bodies { - b.integrateForces() - } - - // Initialize physics manifolds to solve collisions - for _, m := range manifolds { - m.initializeManifolds() - } - - // Integrate physics collisions impulses to solve collisions - manifoldsCount := len(manifolds) - for i := 0; i < collisionIterations; i++ { - for j := 0; j < manifoldsCount; j++ { - if i < manifoldsCount { - manifolds[i].integrateImpulses() - } - } - } - - // Integrate velocity to physics bodies - for _, b := range bodies { - b.integrateVelocity() - } - - // Correct physics bodies positions based on manifolds collision information - for _, m := range manifolds { - m.correctPositions() - } - - // Clear physics bodies forces - for _, b := range bodies { - b.Force = rl.Vector2{} - b.Torque = 0 - } + // Add new body to bodies pointers array and update bodies count + bodies[bodiesCount] = newBody + bodiesCount++ + return newBody } // Reset - Destroys created physics bodies and manifolds func Reset() { - bodies = make([]*Body, 0, maxBodies) - manifolds = make([]*manifold, 0, maxManifolds) -} - -// Close - Unitializes physics pointers -func Close() { - // Unitialize physics manifolds dynamic memory allocations - for _, m := range manifolds { - destroyManifold(m) - } - - // Unitialize physics bodies dynamic memory allocations - for _, b := range bodies { - DestroyBody(b) - } + Close() } // AddForce - Adds a force to a physics body -func (b *Body) AddForce(force rl.Vector2) { - b.Force = rl.Vector2Add(b.Force, force) +func AddForce(body *Body, force rl.Vector2) { + if body != nil { + body.Force = rl.Vector2Add(body.Force, force) + } } // AddTorque - Adds an angular force to a physics body -func (b *Body) AddTorque(amount float32) { - b.Torque += amount +func AddTorque(body *Body, amount float32) { + if body != nil { + body.Torque += amount + } } // Shatter - Shatters a polygon shape physics body to little physics bodies with explosion force -func (b *Body) Shatter(position rl.Vector2, force float32) { - if b.Shape.Type != PolygonShape { +func Shatter(body *Body, position rl.Vector2, force float32) { + if body == nil || body.Shape.Type != PolygonShape { return } - vertexData := b.Shape.VertexData + vertexData := body.Shape.VertexData collision := false for i := 0; i < vertexData.VertexCount; i++ { - positionA := b.Position - positionB := rl.Mat2MultiplyVector2(vertexData.Transform, rl.Vector2Add(b.Position, vertexData.Vertices[i])) - - nextIndex := 0 - if i+1 < vertexData.VertexCount { - nextIndex = i + 1 - } - - positionC := rl.Mat2MultiplyVector2(vertexData.Transform, rl.Vector2Add(b.Position, vertexData.Vertices[nextIndex])) + nextIndex := getNextIndex(i, vertexData.VertexCount) + pA := body.Position + pB := rl.Mat2MultiplyVector2(body.Shape.Transform, + rl.Vector2Add(body.Position, vertexData.Positions[i])) + pC := rl.Mat2MultiplyVector2(body.Shape.Transform, + rl.Vector2Add(body.Position, vertexData.Positions[nextIndex])) // Check collision between each triangle - alpha := ((positionB.Y-positionC.Y)*(position.X-positionC.X) + (positionC.X-positionB.X)*(position.Y-positionC.Y)) / - ((positionB.Y-positionC.Y)*(positionA.X-positionC.X) + (positionC.X-positionB.X)*(positionA.Y-positionC.Y)) - - beta := ((positionC.Y-positionA.Y)*(position.X-positionC.X) + (positionA.X-positionC.X)*(position.Y-positionC.Y)) / - ((positionB.Y-positionC.Y)*(positionA.X-positionC.X) + (positionC.X-positionB.X)*(positionA.Y-positionC.Y)) - + alpha := ((pB.Y-pC.Y)*(position.X-pC.X) + (pC.X-pB.X)*(position.Y-pC.Y)) / + ((pB.Y-pC.Y)*(pA.X-pC.X) + (pC.X-pB.X)*(pA.Y-pC.Y)) + beta := ((pC.Y-pA.Y)*(position.X-pC.X) + (pA.X-pC.X)*(position.Y-pC.Y)) / + ((pB.Y-pC.Y)*(pA.X-pC.X) + (pC.X-pB.X)*(pA.Y-pC.Y)) gamma := 1.0 - alpha - beta if alpha > 0 && beta > 0 && gamma > 0 { @@ -543,207 +424,251 @@ func (b *Body) Shatter(position rl.Vector2, force float32) { } } - if collision { - count := vertexData.VertexCount - bodyPos := b.Position + if !collision { + return + } - vertices := make([]rl.Vector2, count) - trans := vertexData.Transform - for i := 0; i < count; i++ { - vertices[i] = vertexData.Vertices[i] + count := vertexData.VertexCount + bodyPos := body.Position + trans := body.Shape.Transform + + vertices := make([]rl.Vector2, count) + for i := 0; i < count; i++ { + vertices[i] = vertexData.Positions[i] + } + + // Destroy shattered physics body + body.Destroy() + for i := 0; i < count; i++ { + nextIndex := getNextIndex(i, count) + center := triangleBarycenter(vertices[i], vertices[nextIndex], rl.Vector2{}) + center = rl.Vector2Add(bodyPos, center) + offset := rl.Vector2Subtract(center, bodyPos) + + var newBody *Body = NewBodyPolygon(center, 10, 3, 10) + var newData Polygon = Polygon{} + newData.VertexCount = 3 + newData.Positions[0] = rl.Vector2Subtract(vertices[i], offset) + newData.Positions[1] = rl.Vector2Subtract(vertices[nextIndex], offset) + newData.Positions[2] = rl.Vector2Subtract(position, center) + + // Separate vertices to avoid unnecessary physics collisions + newData.Positions[0].X *= 0.95 + newData.Positions[0].Y *= 0.95 + newData.Positions[1].X *= 0.95 + newData.Positions[1].Y *= 0.95 + newData.Positions[2].X *= 0.95 + newData.Positions[2].Y *= 0.95 + + // Calculate polygon faces normals + for j := 0; j < newData.VertexCount; j++ { + nextVertex := getNextIndex(j, newData.VertexCount) + face := rl.Vector2Subtract(newData.Positions[nextVertex], newData.Positions[j]) + + newData.Normals[j] = rl.NewVector2(face.Y, -face.X) + normalize(&newData.Normals[j]) } - // Destroy shattered physics body - DestroyBody(b) + // Apply computed vertex data to new physics body shape + newBody.Shape.VertexData = newData + newBody.Shape.Transform = trans - for i := 0; i < count; i++ { - nextIndex := 0 - if i+1 < count { - nextIndex = i + 1 - } + // Calculate centroid and moment of inertia + center = rl.Vector2{} + area := float32(0.0) + inertia := float32(0.0) - center := triangleBarycenter(vertices[i], vertices[nextIndex], rl.NewVector2(0, 0)) - center = rl.Vector2Add(bodyPos, center) - offset := rl.Vector2Subtract(center, bodyPos) + for j := 0; j < newBody.Shape.VertexData.VertexCount; j++ { + // Triangle vertices, third vertex implied as (0, 0) + nextVertex := getNextIndex(j, newBody.Shape.VertexData.VertexCount) + p1 := newBody.Shape.VertexData.Positions[j] + p2 := newBody.Shape.VertexData.Positions[nextVertex] - newBody := NewBodyPolygon(center, 10, 3, 10) // Create polygon physics body with relevant values + D := rl.Vector2CrossProduct(p1, p2) + triangleArea := D / 2 - newData := Polygon{} - newData.VertexCount = 3 - newData.Transform = trans + area += triangleArea - newData.Vertices[0] = rl.Vector2Subtract(vertices[i], offset) - newData.Vertices[1] = rl.Vector2Subtract(vertices[nextIndex], offset) - newData.Vertices[2] = rl.Vector2Subtract(position, center) + // Use area to weight the centroid average, not just vertex position + center.X += triangleArea * physacK * (p1.X + p2.X) + center.Y += triangleArea * physacK * (p1.Y + p2.Y) - // Separate vertices to avoid unnecessary physics collisions - newData.Vertices[0].X *= 0.95 - newData.Vertices[0].Y *= 0.95 - newData.Vertices[1].X *= 0.95 - newData.Vertices[1].Y *= 0.95 - newData.Vertices[2].X *= 0.95 - newData.Vertices[2].Y *= 0.95 + 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*physacK*D)*intx2 + inty2 + } - // Calculate polygon faces normals - for j := 0; j < newData.VertexCount; j++ { - nextVertex := 0 - if j+1 < newData.VertexCount { - nextVertex = j + 1 - } + center.X *= 1.0 / area + center.Y *= 1.0 / area - face := rl.Vector2Subtract(newData.Vertices[nextVertex], newData.Vertices[j]) + newBody.Mass = area + newBody.InverseMass = safeDiv(1.0, newBody.Mass) + newBody.Inertia = inertia + newBody.InverseInertia = safeDiv(1.0, newBody.Inertia) - newData.Normals[j] = rl.NewVector2(face.Y, -face.X) - normalize(&newData.Normals[j]) - } + // Calculate explosion force direction + pointA := newBody.Position + pointB := rl.Vector2Subtract(newData.Positions[1], newData.Positions[0]) + pointB.X /= 2.0 + pointB.Y /= 2.0 + forceDirection := rl.Vector2Subtract( + rl.Vector2Add(pointA, rl.Vector2Add(newData.Positions[0], pointB)), + newBody.Position, + ) + normalize(&forceDirection) + forceDirection.X *= force + forceDirection.Y *= force - // Apply computed vertex data to new physics body shape - newBody.Shape.VertexData = newData + // Apply force to new physics body + AddForce(newBody, forceDirection) + } +} - // Calculate centroid and moment of inertia - center = rl.NewVector2(0, 0) - area := float32(0.0) - inertia := float32(0.0) - k := float32(1.0) / 3.0 +// GetBodies - Returns the slice of created physics bodies +func GetBodies() []*Body { + return bodies[:] +} - for j := 0; j < newBody.Shape.VertexData.VertexCount; j++ { - // Triangle vertices, third vertex implied as (0, 0) - p1 := newBody.Shape.VertexData.Vertices[j] - nextVertex := 0 - if j+1 < newBody.Shape.VertexData.VertexCount { - nextVertex = j + 1 - } - p2 := newBody.Shape.VertexData.Vertices[nextVertex] +// GetBodiesCount - Returns the current amount of created physics bodies +func GetBodiesCount() int { + return bodiesCount +} - D := rl.Vector2CrossProduct(p1, p2) - triangleArea := D / 2 +// GetBody - Returns a physics body of the bodies pool at a specific index +func GetBody(index int) *Body { + return bodies[index] +} - 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 - - newBody.Mass = area - newBody.Inertia = inertia - - if newBody.Mass != 0 { - newBody.InverseMass = 1.0 / newBody.Mass - } - - if newBody.Inertia != 0 { - newBody.InverseInertia = 1.0 / newBody.Inertia - } - - // Calculate explosion force direction - pointA := newBody.Position - pointB := rl.Vector2Subtract(newData.Vertices[1], newData.Vertices[0]) - pointB.X /= 2 - pointB.Y /= 2 - forceDirection := rl.Vector2Subtract(rl.Vector2Add(pointA, rl.Vector2Add(newData.Vertices[0], pointB)), newBody.Position) - normalize(&forceDirection) - forceDirection.X *= force - forceDirection.Y *= force - - // Apply force to new physics body - newBody.AddForce(forceDirection) +// GetShapeType - Returns the physics body shape type (PHYSICS_CIRCLE or PHYSICS_POLYGON) +func GetShapeType(index int) ShapeType { + result := ShapeType(-1) + if index < bodiesCount { + if bodies[index] != nil { + result = bodies[index].Shape.Type } } + return result +} + +// GetShapeVerticesCount - Returns the amount of vertices of a physics body shape +func GetShapeVerticesCount(index int) int { + var result int = 0 + if index < bodiesCount { + if bodies[index] != nil { + switch bodies[index].Shape.Type { + case CircleShape: + result = circleVertices + case PolygonShape: + result = bodies[index].Shape.VertexData.VertexCount + default: + } + } + } + return result } // GetShapeVertex - Returns transformed position of a body shape (body position + vertex transformed position) func (b *Body) GetShapeVertex(vertex int) rl.Vector2 { - position := rl.Vector2{} + var position rl.Vector2 switch b.Shape.Type { case CircleShape: - position.X = b.Position.X + float32(math.Cos(360/float64(circleVertices)*float64(vertex)*rl.Deg2rad))*b.Shape.Radius - position.Y = b.Position.Y + float32(math.Sin(360/float64(circleVertices)*float64(vertex)*rl.Deg2rad))*b.Shape.Radius - break + angle := 360.0 / circleVertices * float64(vertex) * (degToRad) + position.X = b.Position.X + float32(math.Cos(angle))*b.Shape.Radius + position.Y = b.Position.Y + float32(math.Sin(angle))*b.Shape.Radius case PolygonShape: - position = rl.Vector2Add(b.Position, rl.Mat2MultiplyVector2(b.Shape.VertexData.Transform, b.Shape.VertexData.Vertices[vertex])) - break + position = rl.Vector2Add( + b.Position, + rl.Mat2MultiplyVector2(b.Shape.Transform, b.Shape.VertexData.Positions[vertex]), + ) } - return position } -// SetRotation - Sets physics body shape transform based on radians parameter +// SetBodyRotation - Sets physics body shape transform based on radians parameter func (b *Body) SetRotation(radians float32) { b.Orient = radians - if b.Shape.Type == PolygonShape { - b.Shape.VertexData.Transform = rl.Mat2Radians(radians) + b.Shape.Transform = rl.Mat2Radians(radians) } } -// integrateVelocity - Integrates physics velocity into position and forces -func (b *Body) integrateVelocity() { - if !b.Enabled { +// Destroy - Unitializes and destroy a physics body +func (b *Body) Destroy() { + id := b.ID + index := -1 + for i := 0; i < bodiesCount; i++ { + if bodies[i].ID == id { + index = i + break + } + } + if index == -1 { return } - b.Position.X += b.Velocity.X * deltaTime - b.Position.Y += b.Velocity.Y * deltaTime + bodies[index] = nil - if !b.FreezeOrient { - b.Orient += b.AngularVelocity * deltaTime + // Reorder physics bodies pointers array and its catched index + for i := index; i+1 < bodiesCount; i++ { + bodies[i] = bodies[i+1] } - rl.Mat2Set(&b.Shape.VertexData.Transform, b.Orient) - - b.integrateForces() + // Update physics bodies count + bodiesCount-- } -// integrateForces - Integrates physics forces into velocity -func (b *Body) integrateForces() { - if b.InverseMass == 0 || !b.Enabled { - return +// Close - Unitializes physics pointers +func Close() { + // Unitialize physics manifolds dynamic memory allocations + for i := manifoldsCount - 1; i >= 0; i-- { + destroyManifold(manifolds[i]) } - b.Velocity.X += (b.Force.X * b.InverseMass) * (deltaTime / 2) - b.Velocity.Y += (b.Force.Y * b.InverseMass) * (deltaTime / 2) - - if b.UseGravity { - b.Velocity.X += gravityForce.X * (deltaTime / 1000 / 2) - b.Velocity.Y += gravityForce.Y * (deltaTime / 1000 / 2) - } - - if !b.FreezeOrient { - b.AngularVelocity += b.Torque * b.InverseInertia * (deltaTime / 2) + // Unitialize physics bodies dynamic memory allocations + for i := bodiesCount - 1; i >= 0; i-- { + bodies[i].Destroy() } } -// newRandomPolygon - Creates a random polygon shape with max vertex distance from polygon pivot -func newRandomPolygon(radius float32, sides int) Polygon { - data := Polygon{} +// findAvailableBodyIndex - Finds a valid index for a new physics body initialization +func findAvailableBodyIndex() int { + index := -1 + for i := 0; i < maxBodies; i++ { + currentID := i + + // Check if current id already exist in other physics body + for k := 0; k < bodiesCount; k++ { + if bodies[k].ID == currentID { + currentID++ + break + } + } + + // If it is not used, use it as new physics body id + if currentID == i { + index = i + break + } + } + return index +} + +// createRandomPolygon - Creates a random polygon shape with max vertex distance from polygon pivot +func createRandomPolygon(radius float32, sides int) Polygon { + var data Polygon = Polygon{} data.VertexCount = sides - orient := rl.GetRandomValue(0, 360) - data.Transform = rl.Mat2Radians(float32(orient) * rl.Deg2rad) - // Calculate polygon vertices positions for i := 0; i < data.VertexCount; i++ { - data.Vertices[i].X = float32(math.Cos(360/float64(sides)*float64(i)*rl.Deg2rad)) * radius - data.Vertices[i].Y = float32(math.Sin(360/float64(sides)*float64(i)*rl.Deg2rad)) * radius + data.Positions[i].X = float32(math.Cos(360.0/float64(sides)*float64(i)*degToRad)) * radius + data.Positions[i].Y = float32(math.Sin(360.0/float64(sides)*float64(i)*degToRad)) * radius } // Calculate polygon faces normals for i := 0; i < data.VertexCount; i++ { - nextIndex := 0 - if i+1 < sides { - nextIndex = i + 1 - } - - face := rl.Vector2Subtract(data.Vertices[nextIndex], data.Vertices[i]) + nextIndex := getNextIndex(i, sides) + face := rl.Vector2Subtract(data.Positions[nextIndex], data.Positions[i]) data.Normals[i] = rl.NewVector2(face.Y, -face.X) normalize(&data.Normals[i]) @@ -752,26 +677,21 @@ func newRandomPolygon(radius float32, sides int) Polygon { return data } -// newRectanglePolygon - Creates a rectangle polygon shape based on a min and max positions -func newRectanglePolygon(pos, size rl.Vector2) Polygon { - data := Polygon{} - +// createRectanglePolygon - Creates a rectangle polygon shape based on a min and max positions +func createRectanglePolygon(pos rl.Vector2, size rl.Vector2) Polygon { + var data Polygon = Polygon{} data.VertexCount = 4 - data.Transform = rl.Mat2Radians(0) // Calculate polygon vertices positions - data.Vertices[0] = rl.NewVector2(pos.X+size.X/2, pos.Y-size.Y/2) - data.Vertices[1] = rl.NewVector2(pos.X+size.X/2, pos.Y+size.Y/2) - data.Vertices[2] = rl.NewVector2(pos.X-size.X/2, pos.Y+size.Y/2) - data.Vertices[3] = rl.NewVector2(pos.X-size.X/2, pos.Y-size.Y/2) + data.Positions[0] = rl.NewVector2(pos.X+size.X/2, pos.Y-size.Y/2) + data.Positions[1] = rl.NewVector2(pos.X+size.X/2, pos.Y+size.Y/2) + data.Positions[2] = rl.NewVector2(pos.X-size.X/2, pos.Y+size.Y/2) + data.Positions[3] = rl.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 := rl.Vector2Subtract(data.Vertices[nextIndex], data.Vertices[i]) + nextIndex := getNextIndex(i, data.VertexCount) + face := rl.Vector2Subtract(data.Positions[nextIndex], data.Positions[i]) data.Normals[i] = rl.NewVector2(face.Y, -face.X) normalize(&data.Normals[i]) @@ -780,124 +700,318 @@ func newRectanglePolygon(pos, size rl.Vector2) Polygon { return data } -// newManifold - Creates a new physics manifold to solve collision -func newManifold(a, b *Body) *manifold { - newManifold := &manifold{} +// step - Does physics steps calculations (dynamics, collisions and position corrections) +func step() { + // Clear previous generated collisions information + for i := manifoldsCount - 1; i >= 0; i-- { + if manifold := manifolds[i]; manifold != nil { + destroyManifold(manifold) + } + } + + // 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] + if bodyA == nil { + continue + } + + for j := i + 1; j < bodiesCount; j++ { + var bodyB *Body = bodies[j] + if bodyB == nil || bodyA.InverseMass == 0 && bodyB.InverseMass == 0 { + continue + } + + 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++ { + if body := bodies[i]; body != nil { + integrateForces(body) + } + } + + // Initialize physics manifolds to solve collisions + for i := 0; i < manifoldsCount; i++ { + if manifold := manifolds[i]; manifold != nil { + initializeManifolds(manifold) + } + } + + // Integrate physics collisions impulses to solve collisions + for i := 0; i < collisionIterations; i++ { + for j := 0; j < manifoldsCount; j++ { + if manifold := manifolds[i]; manifold != nil { + integrateImpulses(manifold) + } + } + } + + // Integrate velocity to physics bodies + for i := 0; i < bodiesCount; i++ { + if body := bodies[i]; body != nil { + integrateVelocity(body) + } + } + + // Correct physics bodies positions based on manifolds collision information + for i := 0; i < manifoldsCount; i++ { + if manifold := manifolds[i]; manifold != nil { + correctPositions(manifold) + } + } + + // Clear physics bodies forces + for i := 0; i < bodiesCount; i++ { + if body := bodies[i]; body != nil { + body.Force = rl.Vector2{} + body.Torque = 0 + } + } +} + +// Update - Runs physics step +func Update() { + // Calculate current time + currentTime = getCurrentTime() + + // Calculate current delta time + var delta float32 = currentTime - startTime + + // Store the time elapsed since the last frame began + accumulator += delta + + // Fixed time stepping loop + for accumulator >= deltaTime { + step() + accumulator -= deltaTime + } + + // Record the starting of this frame + startTime = currentTime +} + +// SetTimeStep - Sets physics fixed time step in milliseconds. 1.666666 by default +func SetTimeStep(delta float32) { + deltaTime = delta +} + +// findAvailableManifoldIndex - Finds a valid index for a new manifold initialization +func findAvailableManifoldIndex() int { + index := -1 + for i := 0; i < maxManifolds; i++ { + var currentId int = i + + // Check if current id already exist in other physics body + for k := 0; k < manifoldsCount; k++ { + if manifolds[k].ID == currentId { + currentId++ + break + } + } + + // If it is not used, use it as new physics body id + if currentId == i { + index = i + break + } + } + return index +} + +// createManifold - Creates a new physics manifold to solve collision +func createManifold(a *Body, b *Body) *Manifold { + newID := findAvailableManifoldIndex() + if newID < 0 { + return nil + } // Initialize new manifold with generic values - newManifold.BodyA = a - newManifold.BodyB = b - newManifold.Penetration = 0 - newManifold.Normal = rl.Vector2{} - newManifold.Contacts[0] = rl.Vector2{} - newManifold.Contacts[1] = rl.Vector2{} - newManifold.ContactsCount = 0 - newManifold.Restitution = 0 - newManifold.DynamicFriction = 0 - newManifold.StaticFriction = 0 + newManifold := &Manifold{ + ID: newID, + BodyA: a, + BodyB: b, + Penetration: 0, + Normal: rl.Vector2{}, + Contacts: [2]rl.Vector2{}, + ContactsCount: 0, + Restitution: 0.0, + DynamicFriction: 0.0, + StaticFriction: 0.0, + } - // Add new manifold to manifolds pointers - manifolds = append(manifolds, newManifold) + // Add new contact to conctas pointers array and update contacts count + manifolds[manifoldsCount] = newManifold + manifoldsCount++ return newManifold } // destroyManifold - Unitializes and destroys a physics manifold -func destroyManifold(manifold *manifold) bool { - for index, m := range manifolds { - if m == manifold { - // Free manifold allocated memory - manifolds = append(manifolds[:index], manifolds[index+1:]...) - return true +func destroyManifold(manifold *Manifold) { + if manifold == nil { + return + } + + id := manifold.ID + index := -1 + for i := 0; i < manifoldsCount; i++ { + if manifolds[i].ID == id { + index = i + break + } + } + if index < 0 { + return + } + + manifolds[index] = nil + + // Reorder physics manifolds pointers array and its catched index + for i := index; i < manifoldsCount; i++ { + if (i + 1) < manifoldsCount { + manifolds[i] = manifolds[i+1] } } - return false + // Update physics manifolds count + manifoldsCount-- } // solveManifold - Solves a created physics manifold between two physics bodies -func (m *manifold) solveManifold() { - switch m.BodyA.Shape.Type { +func solveManifold(manifold *Manifold) { + switch manifold.BodyA.Shape.Type { case CircleShape: - switch m.BodyB.Shape.Type { + switch manifold.BodyB.Shape.Type { case CircleShape: - m.solveCircleToCircle() - break + solveCircleToCircle(manifold) case PolygonShape: - m.solveCircleToPolygon() - break + solveCircleToPolygon(manifold) } case PolygonShape: - switch m.BodyB.Shape.Type { + switch manifold.BodyB.Shape.Type { case CircleShape: - m.solvePolygonToCircle() - break + solvePolygonToCircle(manifold) case PolygonShape: - m.solvePolygonToPolygon() - break + solvePolygonToPolygon(manifold) } } - // Update physics body grounded state if normal direction is down and grounded state is not set yet in previous manifolds - if !m.BodyB.IsGrounded { - m.BodyB.IsGrounded = (m.Normal.Y < 0) + // 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 (m *manifold) solveCircleToCircle() { - bodyA := m.BodyA - bodyB := m.BodyB +func solveCircleToCircle(manifold *Manifold) { + bodyA, bodyB := manifold.BodyA, manifold.BodyB + if bodyA == nil || bodyB == nil { + return + } // Calculate translational vector, which is normal - normal := rl.Vector2Subtract(bodyB.Position, bodyA.Position) + var normal rl.Vector2 = rl.Vector2Subtract(bodyB.Position, bodyA.Position) distSqr := rl.Vector2LenSqr(normal) radius := bodyA.Shape.Radius + bodyB.Shape.Radius // Check if circles are not in contact if distSqr >= radius*radius { - m.ContactsCount = 0 + manifold.ContactsCount = 0 return } distance := float32(math.Sqrt(float64(distSqr))) - m.ContactsCount = 1 - + manifold.ContactsCount = 1 if distance == 0 { - m.Penetration = bodyA.Shape.Radius - m.Normal = rl.NewVector2(1, 0) - m.Contacts[0] = bodyA.Position + manifold.Penetration = bodyA.Shape.Radius + manifold.Normal = rl.NewVector2(1, 0) + manifold.Contacts[0] = bodyA.Position } else { - m.Penetration = radius - distance - m.Normal = rl.NewVector2(normal.X/distance, normal.Y/distance) // Faster than using normalize() due to sqrt is already performed - m.Contacts[0] = rl.NewVector2(m.Normal.X*bodyA.Shape.Radius+bodyA.Position.X, m.Normal.Y*bodyA.Shape.Radius+bodyA.Position.Y) + manifold.Penetration = radius - distance + // Faster than using normalize() due to sqrt is already performed + manifold.Normal = rl.NewVector2( + normal.X/distance, + normal.Y/distance, + ) + manifold.Contacts[0] = rl.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 = (m.Normal.Y < 0) + bodyA.IsGrounded = manifold.Normal.Y < 0 } } // solveCircleToPolygon - Solves collision between a circle to a polygon shape physics bodies -func (m *manifold) solveCircleToPolygon() { - m.ContactsCount = 0 +func solveCircleToPolygon(manifold *Manifold) { + bodyA, bodyB := manifold.BodyA, manifold.BodyB + if bodyA == nil || bodyB == nil { + return + } + solveDifferentShapes(manifold, bodyA, bodyB) +} + +// solvePolygonToCircle - Solves collision between a polygon to a circle shape physics bodies +func solvePolygonToCircle(manifold *Manifold) { + bodyA, bodyB := manifold.BodyA, manifold.BodyB + if bodyA == nil || bodyB == nil { + return + } + solveDifferentShapes(manifold, bodyB, bodyA) + manifold.Normal.X *= -1.0 + manifold.Normal.Y *= -1.0 +} + +// solveDifferentShapes - Solves collision between two different types of shapes +func solveDifferentShapes(manifold *Manifold, bodyA *Body, bodyB *Body) { + manifold.ContactsCount = 0 // Transform circle center to polygon transform space - center := m.BodyA.Position - center = rl.Mat2MultiplyVector2(rl.Mat2Transpose(m.BodyB.Shape.VertexData.Transform), rl.Vector2Subtract(center, m.BodyB.Position)) + center := rl.Mat2MultiplyVector2( + rl.Mat2Transpose(bodyB.Shape.Transform), + rl.Vector2Subtract(bodyA.Position, bodyB.Position), + ) // Find edge with minimum penetration - // It is the same concept as using support points in solvePolygonToPolygon + // It is the same concept as using support points in SolvePolygonToPolygon separation := float32(-math.MaxFloat32) faceNormal := 0 - vertexData := m.BodyB.Shape.VertexData + vertexData := bodyB.Shape.VertexData for i := 0; i < vertexData.VertexCount; i++ { - currentSeparation := rl.Vector2DotProduct(vertexData.Normals[i], rl.Vector2Subtract(center, vertexData.Vertices[i])) + currentSeparation := rl.Vector2DotProduct( + vertexData.Normals[i], + rl.Vector2Subtract(center, vertexData.Positions[i]), + ) - if currentSeparation > m.BodyA.Shape.Radius { + if currentSeparation > bodyA.Shape.Radius { return } @@ -908,96 +1022,92 @@ func (m *manifold) solveCircleToPolygon() { } // Grab face's vertices - v1 := vertexData.Vertices[faceNormal] - nextIndex := 0 - if faceNormal+1 < vertexData.VertexCount { - nextIndex = faceNormal + 1 - } - v2 := vertexData.Vertices[nextIndex] + nextIndex := getNextIndex(faceNormal, vertexData.VertexCount) + v1 := vertexData.Positions[faceNormal] + v2 := vertexData.Positions[nextIndex] // Check to see if center is within polygon if separation < epsilon { - m.ContactsCount = 1 - normal := rl.Mat2MultiplyVector2(vertexData.Transform, vertexData.Normals[faceNormal]) - m.Normal = rl.NewVector2(-normal.X, -normal.Y) - m.Contacts[0] = rl.NewVector2(m.Normal.X*m.BodyA.Shape.Radius+m.BodyA.Position.X, m.Normal.Y*m.BodyA.Shape.Radius+m.BodyA.Position.Y) - m.Penetration = m.BodyA.Shape.Radius + manifold.ContactsCount = 1 + var normal rl.Vector2 = rl.Mat2MultiplyVector2(bodyB.Shape.Transform, vertexData.Normals[faceNormal]) + manifold.Normal = rl.NewVector2(-normal.X, -normal.Y) + manifold.Contacts[0] = rl.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 := rl.Vector2DotProduct(rl.Vector2Subtract(center, v1), rl.Vector2Subtract(v2, v1)) dot2 := rl.Vector2DotProduct(rl.Vector2Subtract(center, v2), rl.Vector2Subtract(v1, v2)) - m.Penetration = m.BodyA.Shape.Radius - separation + manifold.Penetration = bodyA.Shape.Radius - separation - if dot1 <= 0 { // Closest to v1 - if rl.Vector2Distance(center, v1) > m.BodyA.Shape.Radius*m.BodyA.Shape.Radius { + switch { + case dot1 <= 0: // Closest to v1 + if rl.Vector2Distance(center, v1) > bodyA.Shape.Radius*bodyA.Shape.Radius { return } - m.ContactsCount = 1 - normal := rl.Vector2Subtract(v1, center) - normal = rl.Mat2MultiplyVector2(vertexData.Transform, normal) + manifold.ContactsCount = 1 + var normal rl.Vector2 = rl.Vector2Subtract(v1, center) + normal = rl.Mat2MultiplyVector2(bodyB.Shape.Transform, normal) normalize(&normal) - m.Normal = normal - v1 = rl.Mat2MultiplyVector2(vertexData.Transform, v1) - v1 = rl.Vector2Add(v1, m.BodyB.Position) - m.Contacts[0] = v1 - } else if dot2 <= 0 { // Closest to v2 - if rl.Vector2Distance(center, v2) > m.BodyA.Shape.Radius*m.BodyA.Shape.Radius { + manifold.Normal = normal + v1 = rl.Mat2MultiplyVector2(bodyB.Shape.Transform, v1) + v1 = rl.Vector2Add(v1, bodyB.Position) + manifold.Contacts[0] = v1 + + case dot2 <= 0: // Closest to v2 + if rl.Vector2Distance(center, v2) > bodyA.Shape.Radius*bodyA.Shape.Radius { return } - m.ContactsCount = 1 - normal := rl.Vector2Subtract(v2, center) - v2 = rl.Mat2MultiplyVector2(vertexData.Transform, v2) - v2 = rl.Vector2Add(v2, m.BodyB.Position) - m.Contacts[0] = v2 - normal = rl.Mat2MultiplyVector2(vertexData.Transform, normal) + manifold.ContactsCount = 1 + var normal rl.Vector2 = rl.Vector2Subtract(v2, center) + v2 = rl.Mat2MultiplyVector2(bodyB.Shape.Transform, v2) + v2 = rl.Vector2Add(v2, bodyB.Position) + manifold.Contacts[0] = v2 + normal = rl.Mat2MultiplyVector2(bodyB.Shape.Transform, normal) normalize(&normal) - m.Normal = normal - } else { // Closest to face - normal := vertexData.Normals[faceNormal] + manifold.Normal = normal - if rl.Vector2DotProduct(rl.Vector2Subtract(center, v1), normal) > m.BodyA.Shape.Radius { + default: // Closest to face + var normal rl.Vector2 = vertexData.Normals[faceNormal] + + if rl.Vector2DotProduct(rl.Vector2Subtract(center, v1), normal) > bodyA.Shape.Radius { return } - normal = rl.Mat2MultiplyVector2(vertexData.Transform, normal) - m.Normal = rl.NewVector2(-normal.X, -normal.Y) - m.Contacts[0] = rl.NewVector2(m.Normal.X*m.BodyA.Shape.Radius+m.BodyA.Position.X, m.Normal.Y*m.BodyA.Shape.Radius+m.BodyA.Position.Y) - m.ContactsCount = 1 + normal = rl.Mat2MultiplyVector2(bodyB.Shape.Transform, normal) + manifold.Normal = rl.NewVector2(-normal.X, -normal.Y) + manifold.Contacts[0] = rl.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 (m *manifold) solvePolygonToCircle() { - bodyA := m.BodyA - bodyB := m.BodyB - - m.BodyA = bodyB - m.BodyB = bodyA - - m.solveCircleToPolygon() - - m.Normal.X *= -1 - m.Normal.Y *= -1 -} - // solvePolygonToPolygon - Solves collision between two polygons shape physics bodies -func (m *manifold) solvePolygonToPolygon() { - bodyA := m.BodyA.Shape - bodyB := m.BodyB.Shape - m.ContactsCount = 0 +func solvePolygonToPolygon(manifold *Manifold) { + bodyA, bodyB := manifold.BodyA, manifold.BodyB + if bodyA == nil || bodyB == nil { + return + } + + shapeA, shapeB := bodyA.Shape, bodyB.Shape + manifold.ContactsCount = 0 // Check for separating axis with A shape's face planes - faceA, penetrationA := findAxisLeastPenetration(bodyA, bodyB) + faceA, penetrationA := findAxisLeastPenetration(shapeA, shapeB) if penetrationA >= 0 { return } // Check for separating axis with B shape's face planes - faceB, penetrationB := findAxisLeastPenetration(bodyB, bodyA) + faceB, penetrationB := findAxisLeastPenetration(shapeB, shapeA) if penetrationB >= 0 { return } @@ -1005,40 +1115,35 @@ func (m *manifold) solvePolygonToPolygon() { referenceIndex := 0 flip := false // Always point from A shape to B shape - refPoly := Shape{} // Reference - incPoly := Shape{} // Incident + var refPoly Shape // Reference + var incPoly Shape // Incident // Determine which shape contains reference face if biasGreaterThan(penetrationA, penetrationB) { - refPoly = bodyA - incPoly = bodyB + refPoly = shapeA + incPoly = shapeB referenceIndex = faceA } else { - refPoly = bodyB - incPoly = bodyA + refPoly = shapeB + incPoly = shapeA referenceIndex = faceB flip = true } // World space incident face - incidentFace0 := rl.Vector2{} - incidentFace1 := rl.Vector2{} - findIncidentFace(&incidentFace0, &incidentFace1, refPoly, incPoly, referenceIndex) + var incidentFace [2]rl.Vector2 + findIncidentFace(&incidentFace[0], &incidentFace[1], refPoly, incPoly, referenceIndex) // Setup reference face vertices refData := refPoly.VertexData - v1 := refData.Vertices[referenceIndex] - if referenceIndex+1 < refData.VertexCount { - referenceIndex = referenceIndex + 1 - } else { - referenceIndex = 0 - } - v2 := refData.Vertices[referenceIndex] + v1 := refData.Positions[referenceIndex] + referenceIndex = getNextIndex(referenceIndex, refData.VertexCount) + v2 := refData.Positions[referenceIndex] // Transform vertices to world space - v1 = rl.Mat2MultiplyVector2(refData.Transform, v1) + v1 = rl.Mat2MultiplyVector2(refPoly.Transform, v1) v1 = rl.Vector2Add(v1, refPoly.Body.Position) - v2 = rl.Mat2MultiplyVector2(refData.Transform, v2) + v2 = rl.Mat2MultiplyVector2(refPoly.Transform, v2) v2 = rl.Vector2Add(v2, refPoly.Body.Position) // Calculate reference face side normal in world space @@ -1048,83 +1153,112 @@ func (m *manifold) solvePolygonToPolygon() { // Orthogonalize refFaceNormal := rl.NewVector2(sidePlaneNormal.Y, -sidePlaneNormal.X) refC := rl.Vector2DotProduct(refFaceNormal, v1) - negSide := rl.Vector2DotProduct(sidePlaneNormal, v1) * -1 + negSide := rl.Vector2DotProduct(sidePlaneNormal, v1) * float32(-1) posSide := rl.Vector2DotProduct(sidePlaneNormal, v2) - // clip incident face to reference face side planes (due to floating point error, possible to not have required points - if clip(rl.NewVector2(-sidePlaneNormal.X, -sidePlaneNormal.Y), negSide, &incidentFace0, &incidentFace1) < 2 { + // Clip incident face to reference face side planes (due to floating point error, possible to not have required points + if clip(rl.NewVector2(-sidePlaneNormal.X, -sidePlaneNormal.Y), negSide, &incidentFace[0], &incidentFace[1]) < 2 { return } - if clip(sidePlaneNormal, posSide, &incidentFace0, &incidentFace1) < 2 { + + if clip(sidePlaneNormal, posSide, &incidentFace[0], &incidentFace[1]) < 2 { return } // Flip normal if required if flip { - m.Normal = rl.NewVector2(-refFaceNormal.X, -refFaceNormal.Y) + manifold.Normal = rl.NewVector2(-refFaceNormal.X, -refFaceNormal.Y) } else { - m.Normal = refFaceNormal + manifold.Normal = refFaceNormal } // Keep points behind reference face - currentPoint := 0 // clipped points behind reference face - separation := rl.Vector2DotProduct(refFaceNormal, incidentFace0) - refC + currentPoint := 0 // Clipped points behind reference face + separation := rl.Vector2DotProduct(refFaceNormal, incidentFace[0]) - refC + if separation <= 0 { - m.Contacts[currentPoint] = incidentFace0 - m.Penetration = -separation + manifold.Contacts[currentPoint] = incidentFace[0] + manifold.Penetration = -separation currentPoint++ } else { - m.Penetration = 0 + manifold.Penetration = 0 } - separation = rl.Vector2DotProduct(refFaceNormal, incidentFace1) - refC + separation = rl.Vector2DotProduct(refFaceNormal, incidentFace[1]) - refC if separation <= 0 { - m.Contacts[currentPoint] = incidentFace1 - m.Penetration += -separation + manifold.Contacts[currentPoint] = incidentFace[1] + manifold.Penetration += -separation currentPoint++ // Calculate total penetration average - m.Penetration /= float32(currentPoint) + manifold.Penetration /= float32(currentPoint) } - m.ContactsCount = currentPoint + manifold.ContactsCount = currentPoint +} + +// integrateForces - Integrates physics forces into velocity +func integrateForces(body *Body) { + if body == nil || body.InverseMass == 0 || !body.Enabled { + return + } + + body.Velocity.X += body.Force.X * body.InverseMass * (deltaTime / 2.0) + body.Velocity.Y += body.Force.Y * body.InverseMass * (deltaTime / 2.0) + + if body.UseGravity { + body.Velocity.X += gravityForce.X * (deltaTime / 1000 / 2.0) + body.Velocity.Y += gravityForce.Y * (deltaTime / 1000 / 2.0) + } + + if !body.FreezeOrient { + body.AngularVelocity += body.Torque * body.InverseInertia * (deltaTime / 2.0) + } } // initializeManifolds - Initializes physics manifolds to solve collisions -func (m *manifold) initializeManifolds() { - bodyA := m.BodyA - bodyB := m.BodyB +func initializeManifolds(manifold *Manifold) { + bodyA, bodyB := manifold.BodyA, manifold.BodyB - // Calculate average restitution, static and dynamic friction - m.Restitution = float32(math.Sqrt(float64(bodyA.Restitution * bodyB.Restitution))) - m.StaticFriction = float32(math.Sqrt(float64(bodyA.StaticFriction * bodyB.StaticFriction))) - m.DynamicFriction = float32(math.Sqrt(float64(bodyA.DynamicFriction * bodyB.DynamicFriction))) + if bodyA == nil || bodyB == nil { + return + } - for i := 0; i < 2; i++ { + // // 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 < manifold.ContactsCount; i++ { // Caculate radius from center of mass to contact - radiusA := rl.Vector2Subtract(m.Contacts[i], bodyA.Position) - radiusB := rl.Vector2Subtract(m.Contacts[i], bodyB.Position) + radiusA := rl.Vector2Subtract(manifold.Contacts[i], bodyA.Position) + radiusB := rl.Vector2Subtract(manifold.Contacts[i], bodyB.Position) crossA := rl.Vector2Cross(bodyA.AngularVelocity, radiusA) crossB := rl.Vector2Cross(bodyB.AngularVelocity, radiusB) - - radiusV := rl.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 + radiusV := rl.NewVector2( + bodyB.Velocity.X+crossB.X-bodyA.Velocity.X-crossA.X, + 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 rl.Vector2LenSqr(radiusV) < (rl.Vector2LenSqr(rl.NewVector2(gravityForce.X*deltaTime/1000, gravityForce.Y*deltaTime/1000)) + epsilon) { - m.Restitution = 0 + // The idea is if the only thing moving this object is gravity, then the collision should be + // performed without any restitution + rad := rl.NewVector2(gravityForce.X*deltaTime/1000, gravityForce.Y*deltaTime/1000) + if rl.Vector2LenSqr(radiusV) < (rl.Vector2LenSqr(rad) + epsilon) { + manifold.Restitution = 0 } } } // integrateImpulses - Integrates physics collisions impulses to solve collisions -func (m *manifold) integrateImpulses() { - bodyA := m.BodyA - bodyB := m.BodyB +func integrateImpulses(manifold *Manifold) { + bodyA, bodyB := manifold.BodyA, manifold.BodyB + + if bodyA == nil || bodyB == nil { + return + } // Early out and positional correct if both objects have infinite mass if math.Abs(float64(bodyA.InverseMass+bodyB.InverseMass)) <= epsilon { @@ -1133,64 +1267,78 @@ func (m *manifold) integrateImpulses() { return } - for i := 0; i < m.ContactsCount; i++ { + for i := 0; i < manifold.ContactsCount; i++ { // Calculate radius from center of mass to contact - radiusA := rl.Vector2Subtract(m.Contacts[i], bodyA.Position) - radiusB := rl.Vector2Subtract(m.Contacts[i], bodyB.Position) + radiusA := rl.Vector2Subtract(manifold.Contacts[i], bodyA.Position) + radiusB := rl.Vector2Subtract(manifold.Contacts[i], bodyB.Position) // Calculate relative velocity - radiusV := rl.Vector2{} - radiusV.X = bodyB.Velocity.X + rl.Vector2Cross(bodyB.AngularVelocity, radiusB).X - bodyA.Velocity.X - rl.Vector2Cross(bodyA.AngularVelocity, radiusA).X - radiusV.Y = bodyB.Velocity.Y + rl.Vector2Cross(bodyB.AngularVelocity, radiusB).Y - bodyA.Velocity.Y - rl.Vector2Cross(bodyA.AngularVelocity, radiusA).Y + radiusV := rl.NewVector2( + bodyB.Velocity.X+rl.Vector2Cross(bodyB.AngularVelocity, radiusB).X- + bodyA.Velocity.X-rl.Vector2Cross(bodyA.AngularVelocity, radiusA).X, + bodyB.Velocity.Y+rl.Vector2Cross(bodyB.AngularVelocity, radiusB).Y- + bodyA.Velocity.Y-rl.Vector2Cross(bodyA.AngularVelocity, radiusA).Y, + ) // Relative velocity along the normal - contactVelocity := rl.Vector2DotProduct(radiusV, m.Normal) + contactVelocity := rl.Vector2DotProduct(radiusV, manifold.Normal) // Do not resolve if velocities are separating if contactVelocity > 0 { return } - raCrossN := rl.Vector2CrossProduct(radiusA, m.Normal) - rbCrossN := rl.Vector2CrossProduct(radiusB, m.Normal) + raCrossN := rl.Vector2CrossProduct(radiusA, manifold.Normal) + rbCrossN := rl.Vector2CrossProduct(radiusB, manifold.Normal) - inverseMassSum := bodyA.InverseMass + bodyB.InverseMass + (raCrossN*raCrossN)*bodyA.InverseInertia + (rbCrossN*rbCrossN)*bodyB.InverseInertia + inverseMassSum := bodyA.InverseMass + bodyB.InverseMass + + (raCrossN*raCrossN)*bodyA.InverseInertia + (rbCrossN*rbCrossN)*bodyB.InverseInertia // Calculate impulse scalar value - impulse := -(1.0 + m.Restitution) * contactVelocity + impulse := -(manifold.Restitution + 1.0) * contactVelocity impulse /= inverseMassSum - impulse /= float32(m.ContactsCount) + impulse /= float32(manifold.ContactsCount) // Apply impulse to each physics body - impulseV := rl.NewVector2(m.Normal.X*impulse, m.Normal.Y*impulse) + impulseV := rl.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 * rl.Vector2CrossProduct(radiusA, rl.NewVector2(-impulseV.X, -impulseV.Y)) + bodyA.AngularVelocity += bodyA.InverseInertia * + rl.Vector2CrossProduct(radiusA, rl.NewVector2(-impulseV.X, -impulseV.Y)) } } if bodyB.Enabled { - bodyB.Velocity.X += bodyB.InverseMass * (impulseV.X) - bodyB.Velocity.Y += bodyB.InverseMass * (impulseV.Y) + bodyB.Velocity.X += bodyB.InverseMass * impulseV.X + bodyB.Velocity.Y += bodyB.InverseMass * impulseV.Y + if !bodyB.FreezeOrient { bodyB.AngularVelocity += bodyB.InverseInertia * rl.Vector2CrossProduct(radiusB, impulseV) } } // Apply friction impulse to each physics body - radiusV.X = bodyB.Velocity.X + rl.Vector2Cross(bodyB.AngularVelocity, radiusB).X - bodyA.Velocity.X - rl.Vector2Cross(bodyA.AngularVelocity, radiusA).X - radiusV.Y = bodyB.Velocity.Y + rl.Vector2Cross(bodyB.AngularVelocity, radiusB).Y - bodyA.Velocity.Y - rl.Vector2Cross(bodyA.AngularVelocity, radiusA).Y + radiusV.X = 0 + + bodyB.Velocity.X + rl.Vector2Cross(bodyB.AngularVelocity, radiusB).X - + bodyA.Velocity.X - rl.Vector2Cross(bodyA.AngularVelocity, radiusA).X + radiusV.Y = 0 + + bodyB.Velocity.Y + rl.Vector2Cross(bodyB.AngularVelocity, radiusB).Y - + bodyA.Velocity.Y - rl.Vector2Cross(bodyA.AngularVelocity, radiusA).Y - tangent := rl.NewVector2(radiusV.X-(m.Normal.X*rl.Vector2DotProduct(radiusV, m.Normal)), radiusV.Y-(m.Normal.Y*rl.Vector2DotProduct(radiusV, m.Normal))) + tangent := rl.NewVector2( + radiusV.X-manifold.Normal.X*rl.Vector2DotProduct(radiusV, manifold.Normal), + radiusV.Y-manifold.Normal.Y*rl.Vector2DotProduct(radiusV, manifold.Normal), + ) normalize(&tangent) // Calculate impulse tangent magnitude - impulseTangent := -(rl.Vector2DotProduct(radiusV, tangent)) + impulseTangent := -rl.Vector2DotProduct(radiusV, tangent) impulseTangent /= inverseMassSum - impulseTangent /= float32(m.ContactsCount) + impulseTangent /= float32(manifold.ContactsCount) absImpulseTangent := float32(math.Abs(float64(impulseTangent))) @@ -1200,11 +1348,14 @@ func (m *manifold) integrateImpulses() { } // Apply coulumb's law - tangentImpulse := rl.Vector2{} - if absImpulseTangent < impulse*m.StaticFriction { + var tangentImpulse rl.Vector2 + if absImpulseTangent < impulse*manifold.StaticFriction { tangentImpulse = rl.NewVector2(tangent.X*impulseTangent, tangent.Y*impulseTangent) } else { - tangentImpulse = rl.NewVector2(tangent.X*-impulse*m.DynamicFriction, tangent.Y*-impulse*m.DynamicFriction) + tangentImpulse = rl.NewVector2( + tangent.X*(-impulse)*manifold.DynamicFriction, + tangent.Y*(-impulse)*manifold.DynamicFriction, + ) } // Apply friction impulse @@ -1213,13 +1364,14 @@ func (m *manifold) integrateImpulses() { bodyA.Velocity.Y += bodyA.InverseMass * (-tangentImpulse.Y) if !bodyA.FreezeOrient { - bodyA.AngularVelocity += bodyA.InverseInertia * rl.Vector2CrossProduct(radiusA, rl.NewVector2(-tangentImpulse.X, -tangentImpulse.Y)) + bodyA.AngularVelocity += bodyA.InverseInertia * + rl.Vector2CrossProduct(radiusA, rl.NewVector2(-tangentImpulse.X, -tangentImpulse.Y)) } } if bodyB.Enabled { - bodyB.Velocity.X += bodyB.InverseMass * (tangentImpulse.X) - bodyB.Velocity.Y += bodyB.InverseMass * (tangentImpulse.Y) + bodyB.Velocity.X += bodyB.InverseMass * tangentImpulse.X + bodyB.Velocity.Y += bodyB.InverseMass * tangentImpulse.Y if !bodyB.FreezeOrient { bodyB.AngularVelocity += bodyB.InverseInertia * rl.Vector2CrossProduct(radiusB, tangentImpulse) @@ -1228,14 +1380,34 @@ func (m *manifold) integrateImpulses() { } } -// correctPositions - Corrects physics bodies positions based on manifolds collision information -func (m *manifold) correctPositions() { - bodyA := m.BodyA - bodyB := m.BodyB +// integrateVelocity - Integrates physics velocity into position and forces +func integrateVelocity(body *Body) { + if body == nil || !body.Enabled { + return + } - correction := rl.Vector2{} - correction.X = float32(math.Max(float64(m.Penetration-penetrationAllowance), 0)) / (bodyA.InverseMass + bodyB.InverseMass) * m.Normal.X * penetrationCorrection - correction.Y = float32(math.Max(float64(m.Penetration-penetrationAllowance), 0)) / (bodyA.InverseMass + bodyB.InverseMass) * m.Normal.Y * penetrationCorrection + body.Position.X += body.Velocity.X * deltaTime + body.Position.Y += body.Velocity.Y * deltaTime + + if !body.FreezeOrient { + body.Orient += body.AngularVelocity * deltaTime + } + + rl.Mat2Set(&body.Shape.Transform, body.Orient) + + integrateForces(body) +} + +// correctPositions - Corrects physics bodies positions based on manifolds collision information +func correctPositions(manifold *Manifold) { + bodyA, bodyB := manifold.BodyA, manifold.BodyB + if bodyA == nil || bodyB == nil { + return + } + + corrCoeff := float32(math.Max(float64(manifold.Penetration-penetrationAllowance), 0)) / + (bodyA.InverseMass + bodyB.InverseMass) * penetrationCorrection + correction := rl.NewVector2(corrCoeff*manifold.Normal.X, corrCoeff*manifold.Normal.Y) if bodyA.Enabled { bodyA.Position.X -= correction.X * bodyA.InverseMass @@ -1254,7 +1426,7 @@ func getSupport(shape Shape, dir rl.Vector2) rl.Vector2 { bestVertex := rl.Vector2{} for i := 0; i < shape.VertexData.VertexCount; i++ { - vertex := shape.VertexData.Vertices[i] + vertex := shape.VertexData.Positions[i] projection := rl.Vector2DotProduct(vertex, dir) if projection > bestProjection { @@ -1267,28 +1439,27 @@ func getSupport(shape Shape, dir rl.Vector2) rl.Vector2 { } // findAxisLeastPenetration - Finds polygon shapes axis least penetration -func findAxisLeastPenetration(shapeA, shapeB Shape) (int, float32) { +func findAxisLeastPenetration(shapeA Shape, shapeB Shape) (int, float32) { bestIndex := 0 bestDistance := float32(-math.MaxFloat32) 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 := rl.Mat2MultiplyVector2(dataA.Transform, normal) + transNormal := rl.Mat2MultiplyVector2(shapeA.Transform, normal) // Transform face normal into B shape's model space - buT := rl.Mat2Transpose(dataB.Transform) + buT := rl.Mat2Transpose(shapeB.Transform) normal = rl.Mat2MultiplyVector2(buT, transNormal) - // Retrieve support point from B shape along -n + // Retrieve vertex on face from A shape, transform into B shape's model space support := getSupport(shapeB, rl.NewVector2(-normal.X, -normal.Y)) // Retrieve vertex on face from A shape, transform into B shape's model space - vertex := dataA.Vertices[i] - vertex = rl.Mat2MultiplyVector2(dataA.Transform, vertex) + vertex := dataA.Positions[i] + vertex = rl.Mat2MultiplyVector2(shapeA.Transform, vertex) vertex = rl.Vector2Add(vertex, shapeA.Body.Position) vertex = rl.Vector2Subtract(vertex, shapeB.Body.Position) vertex = rl.Mat2MultiplyVector2(buT, vertex) @@ -1303,27 +1474,25 @@ func findAxisLeastPenetration(shapeA, shapeB Shape) (int, float32) { } } - return bestIndex, bestDistance + return bestIndex, float32(bestDistance) } // findIncidentFace - Finds two polygon shapes incident face -func findIncidentFace(v0, v1 *rl.Vector2, ref, inc Shape, index int) { +func findIncidentFace(v0 *rl.Vector2, v1 *rl.Vector2, ref Shape, inc Shape, index int) { refData := ref.VertexData incData := inc.VertexData - referenceNormal := refData.Normals[index] + refNormal := refData.Normals[index] // Calculate normal in incident's frame of reference - referenceNormal = rl.Mat2MultiplyVector2(refData.Transform, referenceNormal) // To world space - referenceNormal = rl.Mat2MultiplyVector2(rl.Mat2Transpose(incData.Transform), referenceNormal) // To incident's model space + refNormal = rl.Mat2MultiplyVector2(ref.Transform, refNormal) // To world space + refNormal = rl.Mat2MultiplyVector2(rl.Mat2Transpose(inc.Transform), refNormal) // To incident's model space // Find most anti-normal face on polygon incidentFace := 0 minDot := float32(math.MaxFloat32) - for i := 0; i < incData.VertexCount; i++ { - dot := rl.Vector2DotProduct(referenceNormal, incData.Normals[i]) - + dot := rl.Vector2DotProduct(refNormal, incData.Normals[i]) if dot < minDot { minDot = dot incidentFace = i @@ -1331,26 +1500,17 @@ func findIncidentFace(v0, v1 *rl.Vector2, ref, inc Shape, index int) { } // Assign face vertices for incident face - *v0 = rl.Mat2MultiplyVector2(incData.Transform, incData.Vertices[incidentFace]) + *v0 = rl.Mat2MultiplyVector2(inc.Transform, incData.Positions[incidentFace]) *v0 = rl.Vector2Add(*v0, inc.Body.Position) - - if incidentFace+1 < incData.VertexCount { - incidentFace = incidentFace + 1 - } else { - incidentFace = 0 - } - - *v1 = rl.Mat2MultiplyVector2(incData.Transform, incData.Vertices[incidentFace]) + incidentFace = getNextIndex(incidentFace, incData.VertexCount) + *v1 = rl.Mat2MultiplyVector2(inc.Transform, incData.Positions[incidentFace]) *v1 = rl.Vector2Add(*v1, inc.Body.Position) } // clip - Calculates clipping based on a normal and two faces -func clip(normal rl.Vector2, clip float32, faceA, faceB *rl.Vector2) int { +func clip(normal rl.Vector2, clip float32, faceA *rl.Vector2, faceB *rl.Vector2) int { sp := 0 - - out := make([]rl.Vector2, 2) - out[0] = *faceA - out[1] = *faceB + out := [2]rl.Vector2{*faceA, *faceB} // Retrieve distances from each endpoint to the line distanceA := rl.Vector2DotProduct(normal, *faceA) - clip @@ -1359,11 +1519,12 @@ func clip(normal rl.Vector2, clip float32, faceA, faceB *rl.Vector2) int { // If negative (behind plane) if distanceA <= 0 { out[sp] = *faceA - sp += 1 + sp++ } + if distanceB <= 0 { out[sp] = *faceB - sp += 1 + sp++ } // If the points are on different sides of the plane @@ -1381,38 +1542,61 @@ func clip(normal rl.Vector2, clip float32, faceA, faceB *rl.Vector2) int { // 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 { +// biasGreaterThan - Checks if values are between bias range +func biasGreaterThan(valueA float32, 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 rl.Vector2) rl.Vector2 { - result := rl.Vector2{} - - result.X = (v1.X + v2.X + v3.X) / 3 - result.Y = (v1.Y + v2.Y + v3.Y) / 3 - - return result +func triangleBarycenter(v1 rl.Vector2, v2 rl.Vector2, v3 rl.Vector2) rl.Vector2 { + return rl.NewVector2( + (v1.X+v2.X+v3.X)/3, + (v1.Y+v2.Y+v3.Y)/3, + ) } -// normalize - Normalize provided vector -func normalize(v *rl.Vector2) { - var length, ilength float32 +// initTimer - Initializes hi-resolution MONOTONIC timer +func initTimer() { + rand.Seed(getTimeCount()) + frequency = 1000000000 + startTime = getCurrentTime() // Get current time +} - aux := *v - length = float32(math.Sqrt(float64(aux.X*aux.X + aux.Y*aux.Y))) +// getTimeCount - Gets hi-res MONOTONIC time measure in nanoseconds +func getTimeCount() int64 { + return time.Since(baseTime).Nanoseconds() +} +// getCurrentTime - Gets current time measure in milliseconds +func getCurrentTime() float32 { + return float32(getTimeCount()) / float32(frequency) * 1000 +} + +// normalize - Returns the normalized values of a vector +func normalize(vector *rl.Vector2) { + aux := *vector + length := float32(math.Sqrt(float64(aux.X*aux.X + aux.Y*aux.Y))) if length == 0 { length = 1.0 } - - ilength = 1.0 / length - - v.X *= ilength - v.Y *= ilength + ilength := 1.0 / length + vector.X *= ilength + vector.Y *= ilength +} + +func getNextIndex(i, count int) int { + if i+1 < count { + return i + 1 + } + return 0 +} + +func safeDiv(a, b float32) float32 { + if b == 0 { + return 0 + } + return a / b }