render contact points

deep110 · Jan 5, 2019 · 0cbdce2 · 0cbdce2
1 parent 565c280
commit 0cbdce2
Show file tree

Hide file tree

Showing 6 changed files with 35 additions and 20 deletions.
diff --git a/src/main/java/com/orcus/lucid/physics/CollisionManager.java b/src/main/java/com/orcus/lucid/physics/CollisionManager.java
@@ -101,20 +101,10 @@ private void collisionCircleAABB(Manifold m, RigidBody a, RigidBody b) {
 
             // Find closest axis
             if (StrictMath.abs(n.x) > StrictMath.abs(n.y)) {
+                closest.x = Mathf.sign(closest.x) * x_extent;
+            } else { // y axis is shorter
                 // Clamp to closest extent
-                if (closest.x > 0)
-                    closest.x = x_extent;
-                else
-                    closest.x = -x_extent;
-            }
-
-            // y axis is shorter
-            else {
-                // Clamp to closest extent
-                if (closest.y > 0)
-                    closest.y = y_extent;
-                else
-                    closest.y = -y_extent;
+                closest.y = Mathf.sign(closest.y) * y_extent;
             }
         }
 
@@ -133,7 +123,7 @@ private void collisionCircleAABB(Manifold m, RigidBody a, RigidBody b) {
 
         // Collision normal needs to be flipped to point outside if circle was
         // inside the AABB
-        m.collisionNormal.set((inside) ? n.mul(-1) : n);
+        m.collisionNormal.set((inside) ? n.muli(-1) : n);
         m.penetrationDepth = r - d;
         m.contacts[0].set(a.position).addsi(m.collisionNormal, A.radius);
     }
@@ -164,17 +154,20 @@ private void collisionAABBAABB(Manifold m, RigidBody a, RigidBody b) {
                     sign = Mathf.sign(n.x);
                     m.collisionNormal.set(sign, 0);
                     m.penetrationDepth = x_overlap;
+
+                    Vector2 k = new Vector2(B.width/2, A.height/2).muli(m.collisionNormal);
+                    k.y += n.y;
+                    m.contacts[0].set(a.position).addi(k);
                 } else {
                     // Point toward B knowing that n points from A to B
                     sign = Mathf.sign(n.y);
                     m.collisionNormal.set(0, sign);
                     m.penetrationDepth = y_overlap;
-                }
 
-                // A->position + sign(B-A) * [(w/2, h/2) - normal * pDepth]
-                m.contacts[0].set(a.position)
-                        .addsi(new Vector2(A.width / 2, A.height / 2)
-                                .subsi(m.collisionNormal, m.penetrationDepth), sign);
+                    Vector2 k = new Vector2(B.width/2, A.height/2).muli(m.collisionNormal);
+                    k.x += n.x;
+                    m.contacts[0].set(a.position).addi(k);
+                }
             }
         }
     }

diff --git a/src/main/java/com/orcus/lucid/physics/Manifold.java b/src/main/java/com/orcus/lucid/physics/Manifold.java
@@ -47,7 +47,7 @@ public void initialize() {
             // 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(rv.LenSqr( ) < (dt * gravity).LenSqr( ) + EPSILON)
+            // if(rv.LenSqr() < (dt * gravity).LenSqr() + EPSILON)
             if (rv.lengthSq() < Mathf.RESTING) {
                 e = 0.0f;
             }

diff --git a/src/main/java/com/orcus/lucid/physics/World.java b/src/main/java/com/orcus/lucid/physics/World.java
@@ -2,6 +2,7 @@
 
 import com.orcus.lucid.util.Vector2;
 
+import java.awt.*;
 import java.util.ArrayList;
 
 /**
@@ -95,6 +96,16 @@ public void update(float deltaTime) {
         }
     }
 
+    public void renderContactPoints(Graphics2D gr, float f) {
+        gr.setColor(Color.green);
+        for (Manifold manifold : manifolds) {
+            if (manifold.contactCount > 0) {
+                Vector2 k = manifold.contacts[0].mul(f);
+                gr.drawOval((int)k.x, (int) k.y, 2, 2);
+            }
+        }
+    }
+
     private void generateCollisionInfo() {
         // iterate over every body and save collision info to
         // manifold [O(n2)]

diff --git a/src/main/java/com/orcus/lucid/physics/collider/AABB.java b/src/main/java/com/orcus/lucid/physics/collider/AABB.java
@@ -51,4 +51,9 @@ public void render(RigidBody b, Graphics2D gr, float renderScalingFactor) {
         gr.setColor(Color.blue);
         gr.draw(path);
     }
+
+    @Override
+    public String toString() {
+        return String.format("(%s, %s)", width, height);
+    }
 }
diff --git a/src/main/java/com/orcus/lucid/physics/collider/Circle.java b/src/main/java/com/orcus/lucid/physics/collider/Circle.java
@@ -39,4 +39,9 @@ public void render(RigidBody rigidBody, Graphics2D gr, float renderScalingFactor
         );
 //        gr.draw(new Line2D.Float(b.position.x, b.position.y, b.position.x + rx, b.position.y + ry));
     }
+
+    @Override
+    public String toString() {
+        return Float.toString(radius);
+    }
 }
diff --git a/src/main/java/com/orcus/lucid/render/Scene.java b/src/main/java/com/orcus/lucid/render/Scene.java
@@ -76,6 +76,7 @@ public void renderInternal(GameState state, Graphics2D gr) {
         for (RigidBody b : physicsWorld.getRigidBodies()) {
             b.collider.render(b, gr, METER_TO_PIXEL_MULTIPLIER);
         }
+        physicsWorld.renderContactPoints(gr, METER_TO_PIXEL_MULTIPLIER);
 
         // display graphics to screen
         gameScreen.update();