diff --git a/.classpath b/.classpath index 27d3de9..49e9f61 100644 --- a/.classpath +++ b/.classpath @@ -12,16 +12,6 @@ - - - - - - - - - - @@ -37,5 +27,11 @@ + + + + + + diff --git a/.project b/.project index 1902c26..045956e 100644 --- a/.project +++ b/.project @@ -19,6 +19,5 @@ org.eclipse.jdt.core.javanature - org.python.pydev.pythonNature diff --git a/samples/src/sample/atriasoft/ege/collisiontest/CollisionTestApplication.java b/samples/src/sample/atriasoft/ege/collisiontest/CollisionTestApplication.java index 06911e3..dd9b287 100644 --- a/samples/src/sample/atriasoft/ege/collisiontest/CollisionTestApplication.java +++ b/samples/src/sample/atriasoft/ege/collisiontest/CollisionTestApplication.java @@ -27,7 +27,6 @@ import org.atriasoft.ege.components.PhysicBodyType; import org.atriasoft.ege.engines.EngineLight; import org.atriasoft.ege.engines.EnginePhysics; import org.atriasoft.ege.map.MapVoxel; -import org.atriasoft.ege.physics.shape.Box; import org.atriasoft.ege.tools.MeshGenerator; import org.atriasoft.etk.Color; import org.atriasoft.etk.Uri; @@ -46,6 +45,7 @@ import org.atriasoft.gale.key.KeySpecial; import org.atriasoft.gale.key.KeyStatus; import org.atriasoft.gale.key.KeyType; import org.atriasoft.gale.resource.ResourceColored3DObject; +import org.atriasoft.phyligram.shape.Box; public class CollisionTestApplication extends GaleApplication { public static Vector3f box1HalfSize; diff --git a/samples/src/sample/atriasoft/ege/loxelEngine/LoxelApplication.java b/samples/src/sample/atriasoft/ege/loxelEngine/LoxelApplication.java index a0a0ccc..4d9a544 100644 --- a/samples/src/sample/atriasoft/ege/loxelEngine/LoxelApplication.java +++ b/samples/src/sample/atriasoft/ege/loxelEngine/LoxelApplication.java @@ -28,7 +28,6 @@ import org.atriasoft.ege.components.PhysicBodyType; import org.atriasoft.ege.engines.EngineLight; import org.atriasoft.ege.engines.EnginePhysics; import org.atriasoft.ege.map.MapVoxel; -import org.atriasoft.ege.physics.shape.Box; import org.atriasoft.ege.tools.MeshGenerator; import org.atriasoft.etk.Color; import org.atriasoft.etk.Uri; @@ -47,6 +46,7 @@ import org.atriasoft.gale.key.KeySpecial; import org.atriasoft.gale.key.KeyStatus; import org.atriasoft.gale.key.KeyType; import org.atriasoft.gale.resource.ResourceColored3DObject; +import org.atriasoft.phyligram.shape.Box; public class LoxelApplication extends GaleApplication { public static Vector3f box1HalfSize; diff --git a/samples/src/sample/atriasoft/ege/loxelEnginePerso/LoxelApplicationPerso.java b/samples/src/sample/atriasoft/ege/loxelEnginePerso/LoxelApplicationPerso.java index 15acab3..c7019c2 100644 --- a/samples/src/sample/atriasoft/ege/loxelEnginePerso/LoxelApplicationPerso.java +++ b/samples/src/sample/atriasoft/ege/loxelEnginePerso/LoxelApplicationPerso.java @@ -4,7 +4,6 @@ import java.util.ArrayList; import java.util.List; import org.atriasoft.ege.ControlCameraPlayer; -import org.atriasoft.ege.Engine; import org.atriasoft.ege.Entity; import org.atriasoft.ege.Environement; import org.atriasoft.ege.GameStatus; @@ -26,9 +25,7 @@ import org.atriasoft.ege.components.ComponentStaticMesh; import org.atriasoft.ege.components.ComponentTexture; import org.atriasoft.ege.components.PhysicBodyType; import org.atriasoft.ege.engines.EngineLight; -import org.atriasoft.ege.engines.EnginePhysics; import org.atriasoft.ege.map.MapVoxel; -import org.atriasoft.phyligram.PhysicBox; import org.atriasoft.ege.tools.MeshGenerator; import org.atriasoft.etk.Color; import org.atriasoft.etk.Uri; @@ -47,6 +44,9 @@ import org.atriasoft.gale.key.KeySpecial; import org.atriasoft.gale.key.KeyStatus; import org.atriasoft.gale.key.KeyType; import org.atriasoft.gale.resource.ResourceColored3DObject; +import org.atriasoft.phyligram.PhysicBox; +import org.atriasoft.phyligram.PhysicSphere; +import org.atriasoft.phyligram.PhysicTriangle; public class LoxelApplicationPerso extends GaleApplication { public static Vector3f box1HalfSize; @@ -58,6 +58,7 @@ public class LoxelApplicationPerso extends GaleApplication { public static List testPointsCollide = new ArrayList<>(); public static Quaternion testQTransfert; public static Vector3f testRpos; + public boolean disable = false; private float angleLight = 0; private Quaternion basicRotation = Quaternion.IDENTITY; private Quaternion basicRotation2 = Quaternion.IDENTITY; @@ -107,7 +108,7 @@ public class LoxelApplicationPerso extends GaleApplication { // new Uri("DATA", "basic.vert"), // new Uri("DATA", "basic.frag"))); // env.addEntity(localLight); - { + if (this.disable) { // add a cube to test collision ... final Entity localBox = new Entity(this.env); localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); @@ -117,40 +118,40 @@ public class LoxelApplicationPerso extends GaleApplication { localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0, 0, 5)))); final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env); final PhysicBox box2 = new PhysicBox(); - box2.setSize(new Vector3f(0.5f, 0.5f, 0.5f)); + box2.setSize(new Vector3f(1.001f, 1.001f, 1.001f)); box2.setOrigin(new Vector3f(0, 0, 0)); box2.setMass(1); physics2.addShape(box2); localBox.addComponent(physics2); this.env.addEntity(localBox); } - { + if (this.disable) { // add a cube to test collision ... final Entity localBox = new Entity(this.env); localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine"))); - localBox.addComponent(new ComponentLight(new Light(new Color(0.0f,1.0f, 0.0f), new Vector3f(0, 0, 0), new Vector3f(0.8f, 0.03f, 0.002f)))); + localBox.addComponent(new ComponentLight(new Light(new Color(0.0f, 1.0f, 0.0f), new Vector3f(0, 0, 0), new Vector3f(0.8f, 0.03f, 0.002f)))); localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine"))); - localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0, 4, 12.5f)))); + localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0, 4, 2.5f)))); final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env); final PhysicBox box2 = new PhysicBox(); - box2.setSize(new Vector3f(0.5f, 0.5f, 0.5f)); + box2.setSize(new Vector3f(2.0f, 2.0f, 2.0f)); box2.setOrigin(new Vector3f(0, 0, 0)); box2.setMass(1); physics2.addShape(box2); localBox.addComponent(physics2); this.env.addEntity(localBox); } - { + if (this.disable) { // add a cube to test collision ... final Entity localBox = new Entity(this.env); localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine"))); localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine"))); - localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-2, 2, 14.5f)))); + localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-2, 2, 1.5f)))); final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env); final PhysicBox box2 = new PhysicBox(); - box2.setSize(new Vector3f(0.5f, 0.5f, 0.5f)); + box2.setSize(new Vector3f(3.0f, 3.0f, 3.0f)); box2.setOrigin(new Vector3f(0, 0, 0)); box2.setMass(1); physics2.addShape(box2); @@ -158,13 +159,13 @@ public class LoxelApplicationPerso extends GaleApplication { this.env.addEntity(localBox); } - { + if (this.disable) { // add a cube to test collision ... final Entity localBox = new Entity(this.env); localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine"))); localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine"))); - localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-5, -5, 14)))); + localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-5, -5, 0)))); final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env); final PhysicBox box2 = new PhysicBox(); box2.setSize(new Vector3f(2, 2, 2)); @@ -174,7 +175,7 @@ public class LoxelApplicationPerso extends GaleApplication { localBox.addComponent(physics2); this.env.addEntity(localBox); } - { + if (this.disable) { // add a cube to test collision ... final Entity localBox = new Entity(this.env); Quaternion orientation = new Quaternion(0.5f, 0.2f, 0.4f, 1); @@ -182,7 +183,7 @@ public class LoxelApplicationPerso extends GaleApplication { localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine"))); localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine"))); - localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(15, 15, 14), orientation))); + localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(15, 15, 0), orientation))); final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env); final PhysicBox box2 = new PhysicBox(); box2.setSize(new Vector3f(4, 4, 4)); @@ -192,7 +193,7 @@ public class LoxelApplicationPerso extends GaleApplication { localBox.addComponent(physics2); this.env.addEntity(localBox); } - { + if (this.disable) { // add a cube to test collision ... final Entity localBox = new Entity(this.env); final Quaternion orientation = new Quaternion(0.3f, 1.3f, 0.4f, 1); @@ -200,7 +201,7 @@ public class LoxelApplicationPerso extends GaleApplication { localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine"))); localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine"))); - localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(2, -2, 14.2f), orientation))); + localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(2, -2, 0.2f), orientation))); final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env); // TODO: physics2.setAngularReactionEnable(false); final PhysicBox box2 = new PhysicBox(); @@ -211,7 +212,7 @@ public class LoxelApplicationPerso extends GaleApplication { localBox.addComponent(physics2); this.env.addEntity(localBox); } - { + if (this.disable) { // this is the floor final Entity localBox = new Entity(this.env); Quaternion orientation = new Quaternion(0, 0, 0, 1); @@ -261,6 +262,41 @@ public class LoxelApplicationPerso extends GaleApplication { // env.addEntity(localBox); // } + if (true) { + // add a cube to test collision ... + final Entity localBox = new Entity(this.env); + localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); + localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine"))); + localBox.addComponent(new ComponentLight(new Light(new Color(0.0f, 1.0f, 0.0f), new Vector3f(0, 0, 0), new Vector3f(0.8f, 0.03f, 0.002f)))); + localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine"))); + localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0, 4, 0)))); + final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env); + final PhysicTriangle box2 = new PhysicTriangle(); + box2.setPoints(new Vector3f(2, 0, 0), new Vector3f(0, 2, 0), new Vector3f(3, 3, 2)); + box2.setOrigin(new Vector3f(0, 0, 0)); + box2.setMass(0); + physics2.addShape(box2); + localBox.addComponent(physics2); + this.env.addEntity(localBox); + } + if (true) { + // add a cube to test collision ... + final Entity localBox = new Entity(this.env); + localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); + localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine"))); + localBox.addComponent(new ComponentLight(new Light(new Color(0.0f, 1.0f, 0.0f), new Vector3f(0, 0, 0), new Vector3f(0.8f, 0.03f, 0.002f)))); + localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine"))); + localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(1, 5, 4)))); + final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env); + final PhysicSphere box2 = new PhysicSphere(); + box2.setSize(1.0f); + box2.setOrigin(new Vector3f(0, 0, 0)); + box2.setMass(1); + physics2.addShape(box2); + localBox.addComponent(physics2); + this.env.addEntity(localBox); + } + final Entity gird = new Entity(this.env); gird.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0, 0, 0)))); gird.addComponent(new ComponentStaticMesh(MeshGenerator.createGrid(5))); @@ -304,6 +340,14 @@ public class LoxelApplicationPerso extends GaleApplication { player.addComponent(new ComponentTexture(new Uri("RES", "playerTexture.png"))); player.addComponent(new ComponentRenderTexturedMaterialsStaticMesh(new Uri("DATA", "basicMaterial.vert", "loxelEngine"), new Uri("DATA", "basicMaterial.frag", "loxelEngine"), (EngineLight) this.env.getEngine(EngineLight.ENGINE_NAME))); + final ComponentPhysicsPerso physics = new ComponentPhysicsPerso(this.env); + physics.setBodyType(PhysicBodyType.BODY_DYNAMIC); + final PhysicBox box = new PhysicBox(); + box.setSize(new Vector3f(0.6f, 0.6f, 1.8f)); + box.setOrigin(new Vector3f(0, 0, 0.9f)); + box.setMass(0); + physics.addShape(box); + player.addComponent(physics); this.env.addEntity(player); } final Camera mainView = new Camera(); @@ -320,13 +364,6 @@ public class LoxelApplicationPerso extends GaleApplication { this.basicRotation = Quaternion.fromEulerAngles(new Vector3f(0.005f, 0.005f, 0.01f)); this.basicRotation2 = Quaternion.fromEulerAngles(new Vector3f(0.003f, 0.01f, 0.001f)); - final Engine tmpEngine = this.env.getEngine("physics"); - if (tmpEngine != null) { - final EnginePhysics physicsEngine = (EnginePhysics) tmpEngine; - //Disable gravity for test ... - physicsEngine.setGravity(new Vector3f(0.0f, 0.0f, -9.0f)); - } - // ready to let Gale & Ege manage the display Log.info("==> Init APPL (END)"); } diff --git a/src/module-info.java b/src/module-info.java index 341c3ed..5696f0f 100644 --- a/src/module-info.java +++ b/src/module-info.java @@ -9,9 +9,11 @@ open module org.atriasoft.ege { exports org.atriasoft.ege.engines; exports org.atriasoft.ege.geometry; exports org.atriasoft.ege.map; - exports org.atriasoft.ege.physics.shape; exports org.atriasoft.ege.tools; exports org.atriasoft.phyligram; + exports org.atriasoft.phyligram.shape; + exports org.atriasoft.phyligram.math; + exports org.atriasoft.phyligram.tree; exports entities; exports guis; exports models; diff --git a/src/org/atriasoft/ege/Environement.java b/src/org/atriasoft/ege/Environement.java index 5feee6f..187fdad 100644 --- a/src/org/atriasoft/ege/Environement.java +++ b/src/org/atriasoft/ege/Environement.java @@ -8,13 +8,11 @@ import java.util.List; import java.util.Map; import org.atriasoft.ege.camera.Camera; -import org.atriasoft.ege.components.ComponentPhysicsPerso; import org.atriasoft.ege.engines.EngineAI; import org.atriasoft.ege.engines.EngineDynamicMeshs; import org.atriasoft.ege.engines.EngineGravity; import org.atriasoft.ege.engines.EngineLight; import org.atriasoft.ege.engines.EngineParticle; -import org.atriasoft.ege.engines.EnginePhysics; import org.atriasoft.ege.engines.EnginePhysicsPerso; import org.atriasoft.ege.engines.EnginePlayer; import org.atriasoft.ege.engines.EngineRender; @@ -62,7 +60,7 @@ public class Environement { protected Map listCamera = new HashMap<>(); protected long gameTime = 0; // !< time of the game running - + long startTime = 0; Clock startClock = null; @@ -73,11 +71,11 @@ public class Environement { addEngine(new EngineAI(this)); addEngine(new EngineDynamicMeshs(this)); addEngine(new EngineRender(this)); - addEngine(new EnginePhysics(this)); + //addEngine(new EnginePhysics(this)); addEngine(new EnginePhysicsPerso(this)); addEngine(new EngineParticle(this)); addEngine(new EngineLight(this)); - startClock = Clock.systemUTC(); + this.startClock = Clock.systemUTC(); } /** @@ -221,9 +219,9 @@ public class Environement { // EGEVERBOSE(" Emit Signal"); // signalPlayTimeChange.emit(mgameTime*0.000001f); // } - // + // // //EWOLDEBUG("Time: mlastCallTime=" + mlastCallTime + " deltaTime=" + deltaTime); - // + // // // update camera positions: // for (auto it : mlistCamera) { // if (it.second != null) { @@ -239,7 +237,7 @@ public class Environement { // EGEVERBOSE(" update: " + it.getType()); // it.update(echrono::Duration(double(curentDelta))); // } - // + // // //EGE.debug("stepSimulation (start)"); // ///step the simulation // // TODO mphysicEngine.update(curentDelta); @@ -338,14 +336,15 @@ public class Environement { final long lastUpdate = this.lastCallTime; this.lastCallTime = System.nanoTime(); Clock currentClock = Clock.systemUTC(); - final EventTime event = new EventTime(currentClock, this.startClock, this.lastCallTime , this.startTime, Duration.ofNanos(this.lastCallTime - lastUpdate), Duration.ofNanos(this.lastCallTime - lastUpdate)); + final EventTime event = new EventTime(currentClock, this.startClock, this.lastCallTime, this.startTime, Duration.ofNanos(this.lastCallTime - lastUpdate), + Duration.ofNanos(this.lastCallTime - lastUpdate)); for (final ControlInterface elem : this.controls) { elem.periodicCall(event); } for (final Engine engine : this.engines) { - engine.update((long)((this.lastCallTime - lastUpdate)/1000000)); + engine.update((this.lastCallTime - lastUpdate) / 1000000); } - }; + } public void removeControlInterface(final ControlInterface ref) { this.controls.remove(ref); diff --git a/src/org/atriasoft/ege/components/ComponentPhysics.java b/src/org/atriasoft/ege/components/ComponentPhysics.java index 716922f..bf3bdbe 100644 --- a/src/org/atriasoft/ege/components/ComponentPhysics.java +++ b/src/org/atriasoft/ege/components/ComponentPhysics.java @@ -10,20 +10,11 @@ import org.atriasoft.ege.camera.Camera; import org.atriasoft.ege.components.part.PositionningInterface; import org.atriasoft.ege.engines.EnginePhysics; import org.atriasoft.ege.internal.Log; -import org.atriasoft.ege.physics.shape.Box; -import org.atriasoft.ege.physics.shape.Capsule; -import org.atriasoft.ege.physics.shape.Concave; -import org.atriasoft.ege.physics.shape.Cone; -import org.atriasoft.ege.physics.shape.ConvexHull; -import org.atriasoft.ege.physics.shape.Cylinder; -import org.atriasoft.ege.physics.shape.Shape; -import org.atriasoft.ege.physics.shape.Sphere; import org.atriasoft.ephysics.body.BodyType; import org.atriasoft.ephysics.body.RigidBody; import org.atriasoft.ephysics.collision.ProxyShape; import org.atriasoft.ephysics.collision.TriangleMesh; import org.atriasoft.ephysics.collision.TriangleVertexArray; -import org.atriasoft.ephysics.collision.shapes.AABB; import org.atriasoft.ephysics.collision.shapes.BoxShape; import org.atriasoft.ephysics.collision.shapes.CapsuleShape; import org.atriasoft.ephysics.collision.shapes.CollisionShape; @@ -38,6 +29,14 @@ import org.atriasoft.etk.math.Quaternion; import org.atriasoft.etk.math.Transform3D; import org.atriasoft.etk.math.Vector3f; import org.atriasoft.gale.resource.ResourceColored3DObject; +import org.atriasoft.phyligram.shape.Box; +import org.atriasoft.phyligram.shape.Capsule; +import org.atriasoft.phyligram.shape.Concave; +import org.atriasoft.phyligram.shape.Cone; +import org.atriasoft.phyligram.shape.ConvexHull; +import org.atriasoft.phyligram.shape.Cylinder; +import org.atriasoft.phyligram.shape.Shape; +import org.atriasoft.phyligram.shape.Sphere; public class ComponentPhysics extends Component implements PositionningInterface { public Signal signalPosition = new Signal<>(); @@ -220,62 +219,56 @@ public class ComponentPhysics extends Component implements PositionningInterface public void drawShape(final ResourceColored3DObject _draw, final Camera _camera) { final Transform3D transform = getTransform(); //final float[] mmm = new float[16]; - // Get the OpenGL matrix array of the transform + // Get the OpenGL matrix array of the transform final Matrix4f mmm = transform.getOpenGLMatrix(); final Matrix4f transformationMatrix = mmm.transpose(); final Color tmpColor = new Color(1.0f, 0.0f, 0.0f, 0.3f); for (final Shape it : this.shape) { - if (it.isBox()) { + if (it instanceof Box tmpElement) { Log.debug(" Box"); - final Box tmpElement = (Box) it; final Transform3D transformLocal = new Transform3D(it.getOrigin(), it.getOrientation()); Matrix4f transformationMatrixLocal = transformLocal.getOpenGLMatrix(); transformationMatrixLocal = transformationMatrixLocal.transpose(); transformationMatrixLocal = transformationMatrix.multiply(transformationMatrixLocal); _draw.drawSquare(tmpElement.getSize(), transformationMatrixLocal, tmpColor); - } else if (it.isCylinder()) { + } else if (it instanceof Cylinder tmpElement) { Log.debug(" Cylinder"); - final Cylinder tmpElement = (Cylinder) it; final Transform3D transformLocal = new Transform3D(it.getOrigin(), it.getOrientation()); Matrix4f transformationMatrixLocal = transformLocal.getOpenGLMatrix(); transformationMatrixLocal = transformationMatrixLocal.transpose(); transformationMatrixLocal = transformationMatrix.multiply(transformationMatrixLocal); _draw.drawCylinder(tmpElement.getRadius(), tmpElement.getSize(), 10, 10, transformationMatrixLocal, tmpColor); - } else if (it.isCapsule()) { + } else if (it instanceof Capsule tmpElement) { Log.debug(" Capsule"); - final Capsule tmpElement = (Capsule) it; final Transform3D transformLocal = new Transform3D(it.getOrigin(), it.getOrientation()); Matrix4f transformationMatrixLocal = transformLocal.getOpenGLMatrix(); transformationMatrixLocal = transformationMatrixLocal.transpose(); transformationMatrixLocal = transformationMatrix.multiply(transformationMatrixLocal); _draw.drawCapsule(tmpElement.getRadius(), tmpElement.getSize(), 10, 10, transformationMatrixLocal, tmpColor); - } else if (it.isCone()) { + } else if (it instanceof Cone tmpElement) { Log.debug(" Cone"); - final Cone tmpElement = (Cone) it; final Transform3D transformLocal = new Transform3D(it.getOrigin(), it.getOrientation()); Matrix4f transformationMatrixLocal = transformLocal.getOpenGLMatrix(); transformationMatrixLocal = transformationMatrixLocal.transpose(); transformationMatrixLocal = transformationMatrix.multiply(transformationMatrixLocal); _draw.drawCone(tmpElement.getRadius(), tmpElement.getSize(), 10, 10, transformationMatrixLocal, tmpColor); - } else if (it.isSphere()) { + } else if (it instanceof Sphere tmpElement) { Log.debug(" Sphere"); - final Sphere tmpElement = (Sphere) it; final Transform3D transformLocal = new Transform3D(it.getOrigin(), it.getOrientation()); Matrix4f transformationMatrixLocal = transformLocal.getOpenGLMatrix(); transformationMatrixLocal = transformationMatrixLocal.transpose(); transformationMatrixLocal = transformationMatrix.multiply(transformationMatrixLocal); _draw.drawSphere(tmpElement.getRadius(), 10, 10, transformationMatrixLocal, tmpColor); - } else if (it.isConcave()) { + } else if (it instanceof Concave tmpElement) { Log.debug(" concave"); - final Concave tmpElement = (Concave) it; final Transform3D transformLocal = new Transform3D(it.getOrigin(), it.getOrientation()); Matrix4f transformationMatrixLocal = transformLocal.getOpenGLMatrix(); @@ -283,9 +276,8 @@ public class ComponentPhysics extends Component implements PositionningInterface transformationMatrixLocal = transformationMatrixLocal.multiply(transformationMatrixLocal); _draw.drawTriangles(tmpElement.getVertex(), tmpElement.getIndices(), transformationMatrixLocal, tmpColor); - } else if (it.isConvexHull()) { + } else if (it instanceof ConvexHull tmpElement) { Log.debug(" convexHull"); - final ConvexHull tmpElement = (ConvexHull) it; break; } } @@ -312,9 +304,8 @@ public class ComponentPhysics extends Component implements PositionningInterface if (it == null) { continue; } - if (it.isBox()) { + if (it instanceof Box tmpElement) { Log.debug(" Box"); - final Box tmpElement = (Box) it; // Half extents of the box in the x, y and z directions final Vector3f halfExtents = new Vector3f(tmpElement.getSize().x(), tmpElement.getSize().y(), tmpElement.getSize().z()); // Create the box shape @@ -326,9 +317,8 @@ public class ComponentPhysics extends Component implements PositionningInterface final ProxyShape proxyShape = this.rigidBody.addCollisionShape(shape, transform, it.getMass()); proxyShape.setUserData(this); this.listProxyShape.add(proxyShape); - } else if (it.isCylinder()) { + } else if (it instanceof Cylinder tmpElement) { Log.debug(" Cylinder"); - final Cylinder tmpElement = (Cylinder) it; // Create the Cylinder shape // Create the Cylinder shape final CylinderShape shape = new CylinderShape(tmpElement.getRadius(), tmpElement.getSize()); @@ -338,9 +328,8 @@ public class ComponentPhysics extends Component implements PositionningInterface final ProxyShape proxyShape = this.rigidBody.addCollisionShape(shape, transform, it.getMass()); proxyShape.setUserData(this); this.listProxyShape.add(proxyShape); - } else if (it.isCapsule()) { + } else if (it instanceof Capsule tmpElement) { Log.debug(" Capsule"); - final Capsule tmpElement = (Capsule) it; // Create the Capsule shape final CapsuleShape shape = new CapsuleShape(tmpElement.getRadius(), tmpElement.getSize()); // The ephysic use Y as UP ==> ege use Z as UP @@ -349,9 +338,8 @@ public class ComponentPhysics extends Component implements PositionningInterface final ProxyShape proxyShape = this.rigidBody.addCollisionShape(shape, transform, it.getMass()); proxyShape.setUserData(this); this.listProxyShape.add(proxyShape); - } else if (it.isCone()) { + } else if (it instanceof Cone tmpElement) { Log.debug(" Cone"); - final Cone tmpElement = (Cone) it; // Create the Cone shape final ConeShape shape = new ConeShape(tmpElement.getRadius(), tmpElement.getSize()); // The ephysic use Y as UP ==> ege use Z as UP @@ -360,9 +348,8 @@ public class ComponentPhysics extends Component implements PositionningInterface final ProxyShape proxyShape = this.rigidBody.addCollisionShape(shape, transform, it.getMass()); proxyShape.setUserData(this); this.listProxyShape.add(proxyShape); - } else if (it.isSphere()) { + } else if (it instanceof Sphere tmpElement) { Log.debug(" Sphere"); - final Sphere tmpElement = (Sphere) it; // Create the box shape final SphereShape shape = new SphereShape(tmpElement.getRadius()); // The ephysic use Y as UP ==> ege use Z as UP @@ -371,9 +358,8 @@ public class ComponentPhysics extends Component implements PositionningInterface final ProxyShape proxyShape = this.rigidBody.addCollisionShape(shape, transform, it.getMass()); proxyShape.setUserData(this); this.listProxyShape.add(proxyShape); - } else if (it.isConcave()) { + } else if (it instanceof Concave tmpElement) { Log.debug(" Concave"); - final Concave tmpElement = (Concave) it; //static etk::Vector vertices = {Vector3f(-100.0f,-100.0f,-50.0f),Vector3f(100.0f,-100.0f,-50.0f),Vector3f(100.0f,100.0f,-50.0f)}; //static etk::Vector indices = {0,1,2}; @@ -480,14 +466,13 @@ public class ComponentPhysics extends Component implements PositionningInterface } public void renderDebug(final ResourceColored3DObject _draw, final Camera _camera) { - if (this.rigidBody == null) { - return; - } + if (this.rigidBody == null) {} + /* final Matrix4f transformationMatrix = Matrix4f.IDENTITY; final Color tmpColor = new Color(0.0f, 1.0f, 0.0f, 0.8f); final AABB value = this.rigidBody.getAABB(); _draw.drawCubeLine(value.getMin(), value.getMax(), tmpColor, transformationMatrix, true, true); - + */ } public void setAngularReactionEnable(final boolean value) { diff --git a/src/org/atriasoft/ege/components/ComponentPhysicsPerso.java b/src/org/atriasoft/ege/components/ComponentPhysicsPerso.java index 4df62e6..1cb3045 100644 --- a/src/org/atriasoft/ege/components/ComponentPhysicsPerso.java +++ b/src/org/atriasoft/ege/components/ComponentPhysicsPerso.java @@ -3,50 +3,49 @@ package org.atriasoft.ege.components; import java.util.ArrayList; import java.util.List; +import org.atriasoft.ege.Component; +import org.atriasoft.ege.Environement; +import org.atriasoft.ege.engines.EngineGravity; +import org.atriasoft.ege.engines.EnginePhysicsPerso; +import org.atriasoft.ege.internal.Log; import org.atriasoft.etk.Color; import org.atriasoft.etk.math.Matrix4f; import org.atriasoft.etk.math.Vector3f; import org.atriasoft.gale.resource.ResourceColored3DObject; -import org.atriasoft.ege.internal.Log; -import org.atriasoft.ege.Component; -import org.atriasoft.ege.Environement; -import org.atriasoft.ege.engines.EngineGravity; -import org.atriasoft.ege.engines.EnginePhysics; -import org.atriasoft.ege.engines.EnginePhysicsPerso; +import org.atriasoft.phyligram.ColisionPoint; import org.atriasoft.phyligram.PhysicBox; -import org.atriasoft.phyligram.PhysicCollisionAABB; +import org.atriasoft.phyligram.PhysicHeightMapChunk; import org.atriasoft.phyligram.PhysicMapVoxel; import org.atriasoft.phyligram.PhysicShape; import org.atriasoft.phyligram.PhysicSphere; +import org.atriasoft.phyligram.PhysicTriangle; import org.atriasoft.phyligram.ToolCollisionOBBWithOBB; - +import org.atriasoft.phyligram.ToolCollisionSphereWithHeightMapChunk; +import org.atriasoft.phyligram.ToolCollisionSphereWithSphere; +import org.atriasoft.phyligram.ToolCollisionSphereWithTriangle; +import org.atriasoft.phyligram.shape.AABB; public class ComponentPhysicsPerso extends Component { - private PhysicCollisionAABB aabb; - private List aabbIntersection = new ArrayList(); - private List narrowIntersection = new ArrayList(); - private List shapes = new ArrayList(); + public static float globalMaxSpeed = Float.MAX_VALUE; + private AABB aabb; + private List aabbIntersection = new ArrayList<>(); + private List narrowIntersection = new ArrayList<>(); + List collisionPoints = new ArrayList<>(); + private List shapes = new ArrayList<>(); private ComponentPosition position; private boolean staticObject = false; - private boolean manageGravity = false; - public static float globalMaxSpeed = Float.MAX_VALUE; + private boolean manageGravity = true; private float maxSpeed = globalMaxSpeed; // current speed of the object - private Vector3f speed = new Vector3f(0,0,0); + private Vector3f speed = new Vector3f(0, 0, 0); // current acceleration of the object - private Vector3f acceleration = new Vector3f(0,0,0); + private Vector3f acceleration = new Vector3f(0, 0, 0); // Applied static force on it - private Vector3f staticForce = new Vector3f(0,0,0); + private Vector3f staticForce = new Vector3f(0, 0, 0); // Apply dynamic force on it - private Vector3f dynamicForce = new Vector3f(0,0,0); + private Vector3f dynamicForce = new Vector3f(0, 0, 0); private EnginePhysicsPerso engine; - private PhysicBodyType bodyType; - - - @Override - public String getType() { - return EnginePhysicsPerso.ENGINE_NAME; - } + private PhysicBodyType bodyType; public ComponentPhysicsPerso(final Environement _env) { this.engine = (EnginePhysicsPerso) _env.getEngine(getType()); @@ -56,123 +55,114 @@ public class ComponentPhysicsPerso extends Component { public void addFriendComponent(Component component) { if (component.getType().contentEquals("position")) { if (component instanceof ComponentPosition tmp) { - position = tmp; + this.position = tmp; } else { Log.error("Not manage position model..."); } } } - @Override - public void removeFriendComponent(Component component) { - // nothing to do. + + public void addIntersection(ComponentPhysicsPerso component) { + // do not add multiple times + if (this.aabbIntersection.contains(component)) { + return; + } + this.aabbIntersection.add(component); } - public void updateAABB() { - if (position == null) { - Log.info("No position in Entity "); - return; - } - // TODO Add a flag to check if it is needed to update the AABB... - PhysicCollisionAABB aabbNew = PhysicCollisionAABB.beforeCalculated(); - for (PhysicShape shape : shapes) { - shape.updateAABB(position.getTransform(), aabbNew); - } - aabb = aabbNew; + public void addShape(PhysicShape shape) { + this.shapes.add(shape); } - public PhysicCollisionAABB getAABB() { - return aabb; + public void applyColisionForce() { + for (ColisionPoint impact : this.collisionPoints) { + this.position.applyForce(impact.force); + } + } - public void updateForNarrowCollision() { - narrowIntersection.clear(); - if (aabbIntersection.size() == 0) { - return; + public void applyForces(float timeStep, EngineGravity gravity) { + // get the gravity at the specific position... + Vector3f gravityForce; + if (this.manageGravity) { + gravityForce = gravity.getGravityAtPosition(this.position.getTransform().getPosition()).multiply(timeStep); + } else { + gravityForce = new Vector3f(0, 0, 0); } - if (position == null) { - Log.info("No position in Entity "); - return; - } - for (PhysicShape shape : shapes) { - shape.updateForNarrowCollision(position.getTransform()); + // apply this force on the Object + Log.info("apply gravity: " + gravityForce); + // relative to the object + Vector3f staticForce = this.staticForce.multiply(timeStep); + float globalMass = 0; + for (PhysicShape shape : this.shapes) { + globalMass += shape.getMass(); } + // note the acceleration is not real, it depend on the current delta time... + this.acceleration = gravityForce.add(this.position.getTransform().getOrientation().multiply(staticForce)).add(this.position.getTransform().getOrientation().multiply(this.dynamicForce)) + .multiply(globalMass); + this.dynamicForce = new Vector3f(0, 0, 0); + this.speed = this.speed.add(this.acceleration); + limitWithMaxSpeed(); + Log.info("apply acceleration: " + this.acceleration); + Log.info("apply speed: " + this.speed); + this.position.setTransform(this.position.getTransform().withPosition(this.position.getTransform().getPosition().add(this.speed.multiply(timeStep)))); } - public boolean isNarrowCollide() { - if (narrowIntersection.size() == 0) { - return false; - } - return true; - } - public boolean checkNarrowCollision() { - if (this.staticObject == true) { - return false; - } - for (ComponentPhysicsPerso elem : aabbIntersection) { - boolean collide = false; - for (PhysicShape shapeCurrent : shapes) { - if (elem.checkCollide(shapeCurrent) == true) { - collide = true; - break; - } - } - if (collide == true) { - narrowIntersection.add(elem); - elem.narrowIntersection.add(this); - } - } - return isNarrowCollide(); - } - public void narrowCollisionCreateContactAndForce() { - if (narrowIntersection.size() == 0) { - return; - } - for (ComponentPhysicsPerso elem : narrowIntersection) { - for (PhysicShape shapeCurrent : this.shapes) { - //TODO Do a better method we do this many times ... - if (elem.checkCollide(shapeCurrent) == false) { - continue; - } - elem.getCollidePoints(shapeCurrent, this.staticObject); - } - } - } - + private boolean checkCollide(PhysicShape shapeCurrent) { - if (shapeCurrent instanceof PhysicBox) { - PhysicBox shape111 = (PhysicBox)shapeCurrent; - for (PhysicShape shape : shapes) { - if (shape instanceof PhysicBox) { - PhysicBox shape222 = (PhysicBox)shape; - if (ToolCollisionOBBWithOBB.testCollide(shape111, shape222) == true) { + if (shapeCurrent instanceof PhysicBox shape111) { + for (PhysicShape shape : this.shapes) { + if (shape instanceof PhysicHeightMapChunk shape222) { + // detect collision from cube on height-map !!! + + } else if (shape instanceof PhysicBox shape222) { + // detect collision between 2 cubes + if (ToolCollisionOBBWithOBB.testCollide(shape111, shape222)) { return true; } - } else if (shape instanceof PhysicSphere) { + } else if (shape instanceof PhysicSphere shape222) { - } else if (shape instanceof PhysicMapVoxel) { + } else if (shape instanceof PhysicMapVoxel shape222) { } else { Log.error("Not manage collision model... " + shape); } } - } else if (shapeCurrent instanceof PhysicSphere) { - for (PhysicShape shape : shapes) { - if (shape instanceof PhysicBox) { + } else if (shapeCurrent instanceof PhysicSphere shape111) { + for (PhysicShape shape : this.shapes) { + if (shape instanceof PhysicHeightMapChunk shape222) { + // detect collision from sphere on height-map !!! + if (ToolCollisionSphereWithHeightMapChunk.testCollide(shape111, shape222)) { + return true; + } - } else if (shape instanceof PhysicSphere) { + } else if (shape instanceof PhysicTriangle shape222) { + // detect collision from sphere on height-map !!! + if (ToolCollisionSphereWithTriangle.testCollide(shape111, shape222)) { + return true; + } - } else if (shape instanceof PhysicMapVoxel) { + } else if (shape instanceof PhysicBox shape222) { + // detect collision from sphere on cube !!! + + } else if (shape instanceof PhysicSphere shape222) { + // detect collision from sphere on sphere !!! + if (ToolCollisionSphereWithSphere.testCollide(shape111, shape222)) { + return true; + } + + } else if (shape instanceof PhysicMapVoxel shape222) { } else { Log.error("Not manage collision model... " + shape); } } - } else if (shapeCurrent instanceof PhysicMapVoxel) { - for (PhysicShape shape : shapes) { - if (shape instanceof PhysicBox) { + } else if (shapeCurrent instanceof PhysicMapVoxel shape111) { + for (PhysicShape shape : this.shapes) { + if (shape instanceof PhysicBox shape222) { - } else if (shape instanceof PhysicSphere) { + } else if (shape instanceof PhysicSphere shape222) { - } else if (shape instanceof PhysicMapVoxel) { + } else if (shape instanceof PhysicMapVoxel shape222) { } else { Log.error("Not manage collision model... " + shape); @@ -183,152 +173,204 @@ public class ComponentPhysicsPerso extends Component { } return false; } - private void getCollidePoints(PhysicShape shapeCurrent, boolean isStatic) { - if (shapeCurrent instanceof PhysicBox) { - PhysicBox shape111 = (PhysicBox)shapeCurrent; - for (PhysicShape shape : this.shapes) { - if (shape instanceof PhysicBox) { - PhysicBox shape222 = (PhysicBox)shape; - ToolCollisionOBBWithOBB.getCollidePoints(shape111, isStatic, shape222, this.staticObject); - } else if (shape instanceof PhysicSphere) { - - } else if (shape instanceof PhysicMapVoxel) { - - } else { - Log.error("Not manage collision model... " + shape); - } - } - } else if (shapeCurrent instanceof PhysicSphere) { - for (PhysicShape shape : this.shapes) { - if (shape instanceof PhysicBox) { - - } else if (shape instanceof PhysicSphere) { - - } else if (shape instanceof PhysicMapVoxel) { - - } else { - Log.error("Not manage collision model... " + shape); - } - } - } else if (shapeCurrent instanceof PhysicMapVoxel) { - for (PhysicShape shape : this.shapes) { - if (shape instanceof PhysicBox) { - - } else if (shape instanceof PhysicSphere) { - - } else if (shape instanceof PhysicMapVoxel) { - - } else { - Log.error("Not manage collision model... " + shape); - } - } - } else { - Log.error("Not manage collision model... " + shapeCurrent); + + public boolean checkNarrowCollision() { + if (this.staticObject) { + return false; } - return; + for (ComponentPhysicsPerso elem : this.aabbIntersection) { + boolean collide = false; + for (PhysicShape shapeCurrent : this.shapes) { + if (elem.checkCollide(shapeCurrent)) { + collide = true; + break; + } + } + if (collide) { + this.narrowIntersection.add(elem); + elem.narrowIntersection.add(this); + } + } + return isNarrowCollide(); } - - public void applyForces(float timeStep, EngineGravity gravity) { - // get the gravity at the specific position... - Vector3f gravityForce; - if (manageGravity == true) { - gravityForce = gravity.getGravityAtPosition(position.getTransform().getPosition()).multiply(timeStep); + + public void clearAABBIntersection() { + this.aabbIntersection.clear(); + } + + public void clearPreviousCollision() { + this.collisionPoints.clear(); + this.aabbIntersection.clear(); + this.narrowIntersection.clear(); + } + + public void clearShape() { + this.shapes.clear(); + } + + public AABB getAABB() { + return this.aabb; + } + + public List getAabbIntersection() { + return this.aabbIntersection; + } + + public PhysicBodyType getBodyType() { + return this.bodyType; + } + + private List getCollidePoints(PhysicShape shapeRemote) { + List out = new ArrayList<>(); + if (shapeRemote instanceof PhysicSphere remoteShere) { + for (PhysicShape shape : this.shapes) { + if (shape instanceof PhysicSphere localShape) { + ColisionPoint point = ToolCollisionSphereWithSphere.getCollisionPoint(remoteShere, localShape); + if (point != null) { + out.add(point); + } + } else if (shape instanceof PhysicTriangle localShape) { + ColisionPoint point = ToolCollisionSphereWithTriangle.getCollisionPoint(remoteShere, localShape); + if (point != null) { + out.add(point); + } + + } + } + } else if (shapeRemote instanceof PhysicTriangle) { + // nothing can happens ... } else { - gravityForce = new Vector3f(0,0,0); + Log.error("Not manage collision model... " + shapeRemote); } - // apply this force on the Object - Log.info("apply gravity: " + gravityForce); - // relative to the object - Vector3f staticForce = this.staticForce.multiply(timeStep); - float globalMass = 0; - for (PhysicShape shape : shapes) { - globalMass += shape.getMass(); + return out; + } + + public float getMaxSpeed() { + return this.maxSpeed; + } + + @Override + public String getType() { + return EnginePhysicsPerso.ENGINE_NAME; + } + + public boolean isManageGravity() { + return this.manageGravity; + } + + public boolean isNarrowCollide() { + if (this.narrowIntersection.size() == 0) { + return false; } - // note the acceleration is not real, it depend on the current delta time... - this.acceleration = gravityForce.add(this.position.getTransform().getOrientation().multiply(staticForce)).add(this.position.getTransform().getOrientation().multiply(dynamicForce)).multiply(globalMass); - this.dynamicForce = new Vector3f(0,0,0); - this.speed.add(this.acceleration); - limitWithMaxSpeed(); - Log.info("apply acceleration: " + this.acceleration); - Log.info("apply speed: " + this.speed); - this.position.getTransform().getPosition().add(this.speed.multiply(timeStep)); + return true; + } + + public boolean isStaticObject() { + return this.staticObject; + } + + private void limitWithMaxSpeed() { + if (this.speed.length2() > this.maxSpeed * this.maxSpeed) { + this.speed = this.speed.safeNormalize().multiply(this.maxSpeed); + } + } + + public void narrowCollisionCreateContactAndForce() { + + if (this.staticObject) { + return; + } + if (this.narrowIntersection.size() == 0) { + return; + } + for (ComponentPhysicsPerso elem : this.narrowIntersection) { + for (PhysicShape shapeCurrent : this.shapes) { + //TODO Do a better method we do this many times ... + /* + if (!elem.checkCollide(shapeCurrent)) { + continue; + } + */ + List points = elem.getCollidePoints(shapeCurrent); + this.collisionPoints.addAll(points); + } + } + } + + @Override + public void removeFriendComponent(Component component) { + // nothing to do. } public void renderDebug(ResourceColored3DObject debugDrawProperty) { Color displayColor; + displayColor = new Color(1.0f, 0.0f, 0.0f, 1.0f); + for (ColisionPoint impact : this.collisionPoints) { + debugDrawProperty.drawSquare(new Vector3f(0.02f, 0.02f, 0.02f), Matrix4f.createMatrixTranslate(impact.position), displayColor); + } if (this.aabbIntersection.size() == 0) { - displayColor = new Color(1,1,1,1); + displayColor = new Color(1.0f, 1.0f, 1.0f, 1.0f); } else { if (this.narrowIntersection.size() == 0) { - displayColor = new Color(1,1,0,1); + displayColor = new Color(0.0f, 1.0f, 0.0f, 1.0f); } else { - displayColor = new Color(1,0,0,1); + displayColor = new Color(1.0f, 0.0f, 0.0f, 1.0f); } } - if (aabb != null) { - debugDrawProperty.drawCubeLine(aabb.getMin(), aabb.getMax(), displayColor, Matrix4f.IDENTITY, true, true); + if (this.aabb != null) { + debugDrawProperty.drawCubeLine(this.aabb.getMin(), this.aabb.getMax(), displayColor, Matrix4f.IDENTITY, true, true); //debugDrawProperty.drawCubeLine(new Vector3f(0,0,0), new Vector3f(32,32,32), new Color(1,0,1,1), Matrix4f.identity(), true, true); } else { Log.error("no AABB"); } - for (PhysicShape shape : shapes) { - shape.renderDebug(position.getTransform(), debugDrawProperty); + + for (PhysicShape shape : this.shapes) { + shape.renderDebug(this.position.getTransform(), debugDrawProperty); } + } - public void addShape(PhysicShape shape) { - shapes.add(shape); - } - public void clearShape() { - shapes.clear(); - } - public boolean isManageGravity() { - return manageGravity; - } - public void setManageGravity(boolean manageGravity) { - this.manageGravity = manageGravity; - } - private void limitWithMaxSpeed() { - if (this.speed.length2() > this.maxSpeed*this.maxSpeed) { - this.speed.safeNormalize().multiply(this.maxSpeed); - } - } - public float getMaxSpeed() { - return maxSpeed; - } - - public void setMaxSpeed(float maxSpeed) { - this.maxSpeed = maxSpeed; - } - - public void clearAABBIntersection() { - this.aabbIntersection.clear(); - } - public void addIntersection(ComponentPhysicsPerso component) { - // do not add multiple times - for (ComponentPhysicsPerso elem : this.aabbIntersection) { - if (elem == component) { - return; - } - } - this.aabbIntersection.add(component); - } - public List getAabbIntersection() { - return aabbIntersection; - } - - public boolean isStaticObject() { - return staticObject; - } - - public void setStaticObject(boolean staticObject) { - this.staticObject = staticObject; - } - - public PhysicBodyType getBodyType() { - return bodyType; - } - + public void setBodyType(PhysicBodyType bodyType) { this.bodyType = bodyType; } + + public void setManageGravity(boolean manageGravity) { + this.manageGravity = manageGravity; + } + + public void setMaxSpeed(float maxSpeed) { + this.maxSpeed = maxSpeed; + } + + public void setStaticObject(boolean staticObject) { + this.staticObject = staticObject; + } + + public void updateAABB() { + + if (this.position == null) { + Log.info("No position in Entity "); + return; + } + // TODO Add a flag to check if it is needed to update the AABB... + AABB aabbNew = AABB.createInvertedEmpty(); + for (PhysicShape shape : this.shapes) { + shape.updateAABB(this.position.getTransform(), aabbNew); + } + this.aabb = aabbNew; + } + + public void updateForNarrowCollision() { + this.narrowIntersection.clear(); + if (this.aabbIntersection.size() == 0) { + return; + } + if (this.position == null) { + Log.info("No position in Entity "); + return; + } + for (PhysicShape shape : this.shapes) { + shape.updateForNarrowCollision(this.position.getTransform()); + } + } } diff --git a/src/org/atriasoft/ege/components/ComponentPosition.java b/src/org/atriasoft/ege/components/ComponentPosition.java index 2c9fa08..eb9ce77 100644 --- a/src/org/atriasoft/ege/components/ComponentPosition.java +++ b/src/org/atriasoft/ege/components/ComponentPosition.java @@ -5,9 +5,10 @@ import org.atriasoft.ege.Signal; import org.atriasoft.ege.components.part.PositionningInterface; import org.atriasoft.ege.internal.Log; import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; public class ComponentPosition extends Component implements PositionningInterface { - public final Signal signalPosition = new Signal(); + public final Signal signalPosition = new Signal<>(); protected Transform3D transform; /** @@ -32,6 +33,10 @@ public class ComponentPosition extends Component implements PositionningInterfac } } + public void applyForce(Vector3f force) { + this.transform = this.transform.withPosition(this.transform.getPosition().add(force)); + } + /** * set a new transformation * @return Transformation of the position diff --git a/src/org/atriasoft/ege/engines/EnginePhysics.java b/src/org/atriasoft/ege/engines/EnginePhysics.java index 9e0f5fc..a07b2b4 100644 --- a/src/org/atriasoft/ege/engines/EnginePhysics.java +++ b/src/org/atriasoft/ege/engines/EnginePhysics.java @@ -87,10 +87,9 @@ public class EnginePhysics extends Engine implements EventListener { @Override public void componentAdd(final Component ref) { - if (!(ref instanceof ComponentPhysics)) { + if (!(ref instanceof ComponentPhysics elem)) { return; } - final ComponentPhysics elem = (ComponentPhysics) ref; this.components.add(elem); elem.generate(); } @@ -180,10 +179,8 @@ public class EnginePhysics extends Engine implements EventListener { for (final ContactManifold it : listContact) { for (int iii = 0; iii < it.getNbContactPoints(); iii++) { final ContactPoint contact = it.getContactPoint(iii); - this.debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), Matrix4f.IDENTITY.multiply(Matrix4f.createMatrixTranslate(contact.getWorldPointOnBody1())), - new Color(0, 1, 0, 1)); - this.debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), Matrix4f.IDENTITY.multiply(Matrix4f.createMatrixTranslate(contact.getWorldPointOnBody2())), - new Color(0, 1, 0, 1)); + this.debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), Matrix4f.IDENTITY.multiply(Matrix4f.createMatrixTranslate(contact.getWorldPointOnBody1())), new Color(0, 1, 0, 1)); + this.debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), Matrix4f.IDENTITY.multiply(Matrix4f.createMatrixTranslate(contact.getWorldPointOnBody2())), new Color(0, 1, 0, 1)); } } diff --git a/src/org/atriasoft/ege/engines/EnginePhysicsPerso.java b/src/org/atriasoft/ege/engines/EnginePhysicsPerso.java index ceae58f..974c241 100644 --- a/src/org/atriasoft/ege/engines/EnginePhysicsPerso.java +++ b/src/org/atriasoft/ege/engines/EnginePhysicsPerso.java @@ -2,151 +2,184 @@ package org.atriasoft.ege.engines; import java.util.Vector; -import org.atriasoft.gale.resource.ResourceColored3DObject; -import org.atriasoft.ege.internal.Log; import org.atriasoft.ege.Component; import org.atriasoft.ege.Engine; import org.atriasoft.ege.Environement; import org.atriasoft.ege.camera.Camera; import org.atriasoft.ege.components.ComponentPhysicsPerso; -import org.atriasoft.phyligram.PhysicCollisionAABB; +import org.atriasoft.ege.internal.Log; +import org.atriasoft.gale.resource.ResourceColored3DObject; +import org.atriasoft.phyligram.DebugDisplay; +import org.atriasoft.phyligram.shape.AABB; public class EnginePhysicsPerso extends Engine { public static final String ENGINE_NAME = "physicsPerso"; - private float accumulator = 0; private static final float TIME_STEP = 0.005f; + private float accumulator = 0; private EngineGravity gravity; protected EnginePhysicsPerso engine; - private Vector components = new Vector(); + private Vector components = new Vector<>(); + private Vector componentsWithCollision = new Vector<>(); private ResourceColored3DObject debugDrawProperty = ResourceColored3DObject.create(); public EnginePhysicsPerso(Environement env) { super(env); - this.gravity = (EngineGravity)env.getEngine("gravity"); + this.gravity = (EngineGravity) env.getEngine("gravity"); if (this.gravity == null) { Log.critical("Must initialyse Gravity before physics..."); } } - - @Override - public void componentRemove(Component ref) { - components.remove(ref); + + private void addIncomponentWithCollision(ComponentPhysicsPerso elem) { + if (this.componentsWithCollision.contains(elem)) { + return; + } + this.componentsWithCollision.add(elem); } - + + private void applyForces(float timeStep) { + for (ComponentPhysicsPerso it : this.components) { + it.applyForces(TIME_STEP, this.gravity); + } + } + + /** + * Clear the previous data of collision. + */ + private void clearPreviousCycle() { + for (ComponentPhysicsPerso it : this.components) { + it.clearPreviousCollision(); + } + } + @Override public void componentAdd(Component ref) { if (ref instanceof ComponentPhysicsPerso == false) { return; } - components.add((ComponentPhysicsPerso)ref); + this.components.add((ComponentPhysicsPerso) ref); } - + @Override - public void update(long deltaMili) { - // Add the time difference in the accumulator - accumulator += (float)deltaMili*0.0001f; - // While there is enough accumulated time to take one or several physics steps - while (accumulator >= TIME_STEP) { - Log.info("update physic ... " + accumulator); - //applyForces(TIME_STEP); - updateAABB(TIME_STEP); - updateCollisionsAABB(TIME_STEP); - updateCollisionsNarrowPhase(TIME_STEP); - generateResultCollisionsForces(TIME_STEP); - // Decrease the accumulated time - accumulator -= TIME_STEP; - } - - } - - private void applyForces(float timeStep) { - for (ComponentPhysicsPerso it: components) { - it.applyForces(TIME_STEP, gravity); - } + public void componentRemove(Component ref) { + this.components.remove(ref); } + /** - * Collision detection STEP 1: Upadte the AABB positioning of each elements - * @param timeStep Delta time since the last check - */ - private void updateAABB(float timeStep) { - for (ComponentPhysicsPerso it: components) { - it.updateAABB(); - } - } - /** - * Collision Detection STEP 2: update the list of each element that collide together in the AABB Boxs (update is done between each boxes) - * @param timeStep Delta time since the last check - */ - private void updateCollisionsAABB(float timeStep) { - // clear all object intersection - for (ComponentPhysicsPerso it: components) { - it.clearAABBIntersection(); - } - // update the current object intersection... - for (int iii=0; iii< components.size(); iii++) { - ComponentPhysicsPerso current = components.get(iii); - PhysicCollisionAABB currentAABB = current.getAABB(); - for (int jjj=iii+1; jjj< components.size(); jjj++) { - ComponentPhysicsPerso remote = components.get(jjj); - if (currentAABB.intersect(components.get(jjj).getAABB()) == true) { - current.addIntersection(remote); - remote.addIntersection(current); - } - } - } - } - /** - * Collision Detection STEP 3: Narrow phase: process the collision between every OBB boxes (or other..) - * @param timeStep Delta time since the last check - */ - private void updateCollisionsNarrowPhase(float timeStep) { - // clear all object intersection - for (ComponentPhysicsPerso it: components) { - it.updateForNarrowCollision(); - } - // check for every component if the narrow collision is available. - for (int iii=0; iii< components.size(); iii++) { - ComponentPhysicsPerso current = components.get(iii); - boolean collide = current.checkNarrowCollision(); - - } - // update the force of collision available. - for (int iii=0; iii< components.size(); iii++) { - ComponentPhysicsPerso current = components.get(iii); - current.narrowCollisionCreateContactAndForce(); - } - } - /** - * Collision Detection STEP 4: apply all calculated forces (with containts) - * @param timeStep + * Collision Detection STEP 4: apply all calculated forces (with containts) + * @param timeStep */ private void generateResultCollisionsForces(float timeStep) { - + for (ComponentPhysicsPerso it : this.componentsWithCollision) { + it.applyColisionForce(); + } } - + + @Override + public String getType() { + // TODO Auto-generated method stub + return ENGINE_NAME; + } + @Override public void render(long deltaMili, Camera camera) { // TODO Auto-generated method stub - for (ComponentPhysicsPerso it: this.components) { + for (ComponentPhysicsPerso it : this.components) { //Log.info("Render " + it); - it.renderDebug(debugDrawProperty); + it.renderDebug(this.debugDrawProperty); } //debugDrawProperty.drawCone(2, 5, 9, 12, Matrix4f.identity(), new Color(1,1,0,1)); //debugDrawProperty.drawSquare(new Vector3f(1,1,1), Matrix4f.identity(), new Color(1,1,0,1)); //debugDrawProperty.drawCubeLine(new Vector3f(1,1,1), new Vector3f(5,5,5), new Color(1,0,1,1), Matrix4f.identity(), true, true); //debugDrawProperty.drawCubeLine(new Vector3f(0,0,0), new Vector3f(32,32,32), new Color(1,0,1,1), Matrix4f.identity(), true, true); } - + @Override public void renderDebug(long deltaMili, Camera camera) { // TODO Auto-generated method stub + DebugDisplay.onDraw(); + DebugDisplay.clear(); + } + + @Override + public void update(long deltaMili) { + // Add the time difference in the accumulator + this.accumulator += deltaMili * 0.0001f; + // While there is enough accumulated time to take one or several physics steps + while (this.accumulator >= TIME_STEP) { + Log.info("update physic ... " + this.accumulator); + clearPreviousCycle(); + applyForces(TIME_STEP); + // update AABB after because in rotation force, the Bounding box change... + updateAABB(TIME_STEP); + // update the colision tree between each object in the room + updateCollisionsAABB(TIME_STEP); + updateCollisionsNarrowPhase(TIME_STEP); + generateResultCollisionsForces(TIME_STEP); + // Decrease the accumulated time + this.accumulator -= TIME_STEP; + } } - - @Override - public String getType() { - // TODO Auto-generated method stub - return ENGINE_NAME; + + /** + * Collision detection STEP 1: Upadte the AABB positioning of each elements + * @param timeStep Delta time since the last check + */ + private void updateAABB(float timeStep) { + for (ComponentPhysicsPerso it : this.components) { + it.updateAABB(); + } } - + + /** + * Collision Detection STEP 2: update the list of each element that collide together in the AABB Boxs (update is done between each boxes) + * @param timeStep Delta time since the last check + */ + // TODO : generate a B-TREE to manage collision, it is faster, but now, this is not the purpose ... + private void updateCollisionsAABB(float timeStep) { + this.componentsWithCollision.clear(); + // clear all object intersection + for (ComponentPhysicsPerso it : this.components) { + it.clearAABBIntersection(); + } + // update the current object intersection... + for (int iii = 0; iii < this.components.size(); iii++) { + ComponentPhysicsPerso current = this.components.get(iii); + AABB currentAABB = current.getAABB(); + for (int jjj = iii + 1; jjj < this.components.size(); jjj++) { + ComponentPhysicsPerso remote = this.components.get(jjj); + // prefer checking the collision, this a time-constant operation, check if collision already exist is a unpredictable time. + if (currentAABB.intersect(this.components.get(jjj).getAABB()) == true) { + current.addIntersection(remote); + remote.addIntersection(current); + addIncomponentWithCollision(remote); + addIncomponentWithCollision(current); + } + } + } + } + + /** + * Collision Detection STEP 3: Narrow phase: process the collision between every OBB boxes (or other..) + * @param timeStep Delta time since the last check + */ + private void updateCollisionsNarrowPhase(float timeStep) { + // clear all object intersection + for (ComponentPhysicsPerso it : this.componentsWithCollision) { + it.updateForNarrowCollision(); + } + // check for every component if the narrow collision is available. + for (int iii = 0; iii < this.componentsWithCollision.size(); iii++) { + ComponentPhysicsPerso current = this.componentsWithCollision.get(iii); + boolean collide = current.checkNarrowCollision(); + + } + // update the force of collision available. + for (int iii = 0; iii < this.components.size(); iii++) { + ComponentPhysicsPerso current = this.components.get(iii); + current.narrowCollisionCreateContactAndForce(); + } + } + } diff --git a/src/org/atriasoft/ege/geometry/Geometry3D.java b/src/org/atriasoft/ege/geometry/Geometry3D.java index ebe2e69..c20b6da 100644 --- a/src/org/atriasoft/ege/geometry/Geometry3D.java +++ b/src/org/atriasoft/ege/geometry/Geometry3D.java @@ -52,7 +52,7 @@ public class Geometry3D { return true; } - public static boolean pointInPlane(final Vector3f point, final Plane plane) { + public static boolean pointInPlane(final Vector3f point, final Plane____ plane) { // This should probably use an epsilon! //return Dot(point, plane.normal) - plane.distance == 0.0f; return CMP(point.dot(plane.normal) - plane.distance, 0.0f); diff --git a/src/org/atriasoft/ege/geometry/Plane.java b/src/org/atriasoft/ege/geometry/Plane____.java similarity index 78% rename from src/org/atriasoft/ege/geometry/Plane.java rename to src/org/atriasoft/ege/geometry/Plane____.java index 9ad0dcc..e295577 100644 --- a/src/org/atriasoft/ege/geometry/Plane.java +++ b/src/org/atriasoft/ege/geometry/Plane____.java @@ -2,15 +2,15 @@ package org.atriasoft.ege.geometry; import org.atriasoft.etk.math.Vector3f; -public class Plane { +public class Plane____ { public Vector3f normal; public float distance; - public Plane(Vector3f normal, float distance) { + public Plane____(Vector3f normal, float distance) { this.normal = normal; this.distance = distance; } - public Plane() { + public Plane____() { this.normal = new Vector3f(1.0f, 0.0f, 0.0f); this.distance = 0; } diff --git a/src/org/atriasoft/ege/geometry/Triangle.java b/src/org/atriasoft/ege/geometry/Triangle.java index df80fbf..ab68c4a 100644 --- a/src/org/atriasoft/ege/geometry/Triangle.java +++ b/src/org/atriasoft/ege/geometry/Triangle.java @@ -3,8 +3,19 @@ package org.atriasoft.ege.geometry; import org.atriasoft.etk.math.Vector3f; public class Triangle { + public static Vector3f getCenter(Vector3f p1, Vector3f p2, Vector3f p3) { + return p1.add(p2).add(p3).multiply(0.33333333333f); + } + + public static Vector3f getNormal(Vector3f p1, Vector3f p2, Vector3f p3) { + Vector3f dir = p2.less(p1).cross(p3.less(p1)); + return dir.normalize(); + } + public Vector3f p1; + public Vector3f p2; + public Vector3f p3; public Triangle() { @@ -19,9 +30,26 @@ public class Triangle { this.p3 = p3; } + @Override + public Triangle clone() { + return new Triangle(this.p1, this.p2, this.p3); + } + + public Vector3f getCenter() { + return this.p1.add(this.p2).add(this.p3).multiply(0.33333333333f); + } + + public Vector3f getNormal() { + Vector3f dir = this.p2.less(this.p1).cross(this.p3.less(this.p1)); + return dir.normalize(); + } + + public Plane getPlane() { + return new Plane(getNormal(), getCenter()); + } + @Override public String toString() { return "Triangle [p1=" + this.p1 + ", p2=" + this.p2 + ", p3=" + this.p3 + "]"; } - } diff --git a/src/org/atriasoft/ege/samples/todo_sample.md b/src/org/atriasoft/ege/todo_sample.md similarity index 100% rename from src/org/atriasoft/ege/samples/todo_sample.md rename to src/org/atriasoft/ege/todo_sample.md diff --git a/src/org/atriasoft/phyligram/ColisionPoints.java b/src/org/atriasoft/phyligram/ColisionPoint.java similarity index 66% rename from src/org/atriasoft/phyligram/ColisionPoints.java rename to src/org/atriasoft/phyligram/ColisionPoint.java index 7c97d06..7b0f091 100644 --- a/src/org/atriasoft/phyligram/ColisionPoints.java +++ b/src/org/atriasoft/phyligram/ColisionPoint.java @@ -2,11 +2,11 @@ package org.atriasoft.phyligram; import org.atriasoft.etk.math.Vector3f; -public class ColisionPoints { +public class ColisionPoint { public Vector3f position; public Vector3f force; - public ColisionPoints(Vector3f position, Vector3f force) { + public ColisionPoint(Vector3f position, Vector3f force) { this.position = position; this.force = force; } diff --git a/src/org/atriasoft/phyligram/Collision.java b/src/org/atriasoft/phyligram/Collision.java index 6248da5..b76d386 100644 --- a/src/org/atriasoft/phyligram/Collision.java +++ b/src/org/atriasoft/phyligram/Collision.java @@ -1,12 +1,12 @@ package org.atriasoft.phyligram; public class Collision { - public final ColisionPoints[] colisionPointLocal; + public final ColisionPoint[] colisionPointLocal; public final PhysicShape shapeRemote; - public final ColisionPoints[] colisionPointRemote; + public final ColisionPoint[] colisionPointRemote; public final boolean staticRemote; - public Collision(ColisionPoints[] colisionPointLocal, PhysicShape shapeRemote, - ColisionPoints[] colisionPointRemote, boolean staticRemote) { + public Collision(ColisionPoint[] colisionPointLocal, PhysicShape shapeRemote, + ColisionPoint[] colisionPointRemote, boolean staticRemote) { super(); this.colisionPointLocal = colisionPointLocal; this.shapeRemote = shapeRemote; diff --git a/src/org/atriasoft/phyligram/DebugDisplay.java b/src/org/atriasoft/phyligram/DebugDisplay.java index abda111..b55caf1 100644 --- a/src/org/atriasoft/phyligram/DebugDisplay.java +++ b/src/org/atriasoft/phyligram/DebugDisplay.java @@ -8,303 +8,62 @@ import org.atriasoft.etk.math.Matrix4f; import org.atriasoft.etk.math.Quaternion; import org.atriasoft.etk.math.Vector3f; import org.atriasoft.gale.backend3d.OpenGL; -import org.atriasoft.gale.context.GaleContext; import org.atriasoft.gale.resource.ResourceColored3DObject; public class DebugDisplay { -// public static ComponentPosition relativeTestPos; -// public static PhysicBox boxTest; - public static List testPoints = new ArrayList(); - public static List testPointsBox = new ArrayList(); - public static List testPointsCollide = new ArrayList(); + // public static ComponentPosition relativeTestPos; + // public static PhysicBox boxTest; + public static List testPoints = new ArrayList<>(); + public static List testPointsBox = new ArrayList<>(); + public static List testPointsCollide = new ArrayList<>(); public static Vector3f testRpos; public static Quaternion testQTransfert; public static Vector3f box1HalfSize; public static Vector3f box2HalfSize; - private ResourceColored3DObject debugDrawProperty; - public DebugDisplay(){ + private static ResourceColored3DObject debugDrawProperty; + + static { + if (debugDrawProperty == null) { + debugDrawProperty = ResourceColored3DObject.create(); + } } - /* - @Override - public void onCreate(Context context) { - // set the system global max speed - ComponentPhysics.globalMaxSpeed = 3; - Gale.getContext().grabPointerEvents(true, new Vector2f(0,0)); - env = new Environement(); - this.canDraw = true; - setSize(new Vector2f(1500, 1500)); - setTitle("Loxel sample"); - map = new MapVoxel(this.env); -// this.env.addEngine(map); -// map.init(); - - // simple sun to have a global light ... - Entity globalGravity = new Entity(this.env); - globalGravity.addComponent(new ComponentGravityStatic(new Vector3f(0,0,-1))); - env.addEntity(globalGravity); + + public static void clear() { + testPoints.clear(); + testPointsBox.clear(); + testPointsCollide.clear(); - // simple sun to have a global light ... - Entity sun = new Entity(this.env); - sun.addComponent(new ComponentPosition(new Transform3D(new Vector3f(1000,1000,1000)))); - sun.addComponent(new ComponentLightSun(new Light(new Vector3f(0.4f,0.4f,0.4f), new Vector3f(0,0,0), new Vector3f(0.8f,0,0)))); - env.addEntity(sun); - - // add a cube to show where in the light ... - Entity localLight = new Entity(this.env); - lightPosition = new ComponentPosition(new Transform3D(new Vector3f(-10,-10,17))); -// localLight.addComponent(lightPosition); -// localLight.addComponent(new ComponentStaticMesh(new Uri("RES", "cube.obj"))); -// localLight.addComponent(new ComponentTexture(new Uri("RES", "grass.png"))); -// localLight.addComponent(new ComponentLight(new Light(new Vector3f(0,1,0), new Vector3f(0,0,0), new Vector3f(0.8f,0.03f,0.002f)))); -// localLight.addComponent(new ComponentRenderTexturedStaticMesh( -// new Uri("DATA", "basic.vert"), -// new Uri("DATA", "basic.frag"))); -// env.addEntity(localLight); - { - // add a cube to test collision ... - Entity localBox = new Entity(this.env); - localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-2,-2,14)))); - localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); - localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png"))); - localBox.addComponent(new ComponentLight(new Light(new Vector3f(0,1,0), new Vector3f(0,0,0), new Vector3f(0.8f,0.03f,0.002f)))); - localBox.addComponent(new ComponentRenderTexturedStaticMesh( - new Uri("DATA", "basic.vert"), - new Uri("DATA", "basic.frag"))); - ComponentPhysics physics2 = new ComponentPhysics(true); - PhysicBox box2 = new PhysicBox(); - box2.setSize(new Vector3f(1,1,1)); - box2.setOrigin(new Vector3f(0,0,0)); - box2.setMass(1); - physics2.addShape(box2); - localBox.addComponent(physics2); - env.addEntity(localBox); - } - { - // add a cube to test collision ... - Entity localBox = new Entity(this.env); - localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0,4,12.5f)))); - localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); - localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png"))); - localBox.addComponent(new ComponentLight(new Light(new Vector3f(0,1,0), new Vector3f(0,0,0), new Vector3f(0.8f,0.03f,0.002f)))); - localBox.addComponent(new ComponentRenderTexturedStaticMesh( - new Uri("DATA", "basic.vert"), - new Uri("DATA", "basic.frag"))); - ComponentPhysics physics2 = new ComponentPhysics(true); - PhysicBox box2 = new PhysicBox(); - box2.setSize(new Vector3f(1,1,1)); - box2.setOrigin(new Vector3f(0,0,0)); - box2.setMass(1); - physics2.addShape(box2); - localBox.addComponent(physics2); - env.addEntity(localBox); - } - { - // add a cube to test collision ... - Entity localBox = new Entity(this.env); - localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-2,2,14.5f)))); - localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); - localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png"))); - localBox.addComponent(new ComponentRenderTexturedStaticMesh( - new Uri("DATA", "basic.vert"), - new Uri("DATA", "basic.frag"))); - ComponentPhysics physics2 = new ComponentPhysics(true); - PhysicBox box2 = new PhysicBox(); - box2.setSize(new Vector3f(1,1,1)); - box2.setOrigin(new Vector3f(0,0,0)); - box2.setMass(1); - physics2.addShape(box2); - localBox.addComponent(physics2); - env.addEntity(localBox); - } - - { - // add a cube to test collision ... - Entity localBox = new Entity(this.env); - localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-5,-5,14)))); - localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); - localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png"))); - localBox.addComponent(new ComponentRenderTexturedStaticMesh( - new Uri("DATA", "basic.vert"), - new Uri("DATA", "basic.frag"))); - ComponentPhysics physics2 = new ComponentPhysics(true); - PhysicBox box2 = new PhysicBox(); - box2.setSize(new Vector3f(4,4,4)); - box2.setOrigin(new Vector3f(0,0,0)); - box2.setMass(1); - physics2.addShape(box2); - localBox.addComponent(physics2); - env.addEntity(localBox); - } - { - // add a cube to test collision ... - Entity localBox = new Entity(this.env); - Quaternion transform = new Quaternion(0.5f,0.2f,0.4f,1); - transform.normalize(); - localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(15,15,14), transform))); - localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); - localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png"))); - localBox.addComponent(new ComponentRenderTexturedStaticMesh( - new Uri("DATA", "basic.vert"), - new Uri("DATA", "basic.frag"))); - ComponentPhysics physics2 = new ComponentPhysics(true); - PhysicBox box2 = new PhysicBox(); - box2.setSize(new Vector3f(8,8,8)); - box2.setOrigin(new Vector3f(0,0,0)); - box2.setMass(1); - physics2.addShape(box2); - localBox.addComponent(physics2); - env.addEntity(localBox); - } - { - // add a cube to test collision ... - Entity localBox = new Entity(this.env); - Quaternion transform = new Quaternion(0.3f,0.3f,0.4f,1); - transform.normalize(); - localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(2,-2,14.2f),transform))); - localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); - localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png"))); - localBox.addComponent(new ComponentRenderTexturedStaticMesh( - new Uri("DATA", "basic.vert"), - new Uri("DATA", "basic.frag"))); - ComponentPhysics physics2 = new ComponentPhysics(true); - PhysicBox box2 = new PhysicBox(); - box2.setSize(new Vector3f(1,1,1)); - box2.setOrigin(new Vector3f(0,0,0)); - box2.setMass(1); - physics2.addShape(box2); - localBox.addComponent(physics2); - env.addEntity(localBox); - } - { - // add a cube to test collision ... - Entity localBox = new Entity(this.env); - Quaternion transform = new Quaternion(0,0,1,1); - transform.normalize(); - localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(2,2,14.2f),transform))); - localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); - localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png"))); - localBox.addComponent(new ComponentRenderTexturedStaticMesh( - new Uri("DATA", "basic.vert"), - new Uri("DATA", "basic.frag"))); - ComponentPhysics physics2 = new ComponentPhysics(true); - PhysicBox box2 = new PhysicBox(); - box2.setSize(new Vector3f(1,1,1)); - box2.setOrigin(new Vector3f(0,0,0)); - box2.setMass(1); - physics2.addShape(box2); - localBox.addComponent(physics2); - env.addEntity(localBox); - } -// { -// // add a cube to test collision ... -// Entity localBox = new Entity(this.env); -// relativeTestPos = new ComponentPosition(new Transform3D(new Vector3f(0,0,14),new Quaternion(0.5f,0.2f,0.4f,1))); -// localBox.addComponent(relativeTestPos); -//// localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); -//// localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png"))); -//// localBox.addComponent(new ComponentLight(new Light(new Vector3f(0,1,0), new Vector3f(0,0,0), new Vector3f(0.8f,0.03f,0.002f)))); -//// localBox.addComponent(new ComponentRenderTexturedStaticMesh( -//// new Uri("DATA", "basic.vert"), -//// new Uri("DATA", "basic.frag"))); -// ComponentPhysics physics2 = new ComponentPhysics(true); -// boxTest = new PhysicBox(); -// boxTest.setSize(new Vector3f(1,1,1)); -// boxTest.setOrigin(new Vector3f(0,0,0)); -// boxTest.setMass(1); -// physics2.addShape(boxTest); -// localBox.addComponent(physics2); -// env.addEntity(localBox); -// } -// { -// Entity localBox = new Entity(this.env); -// localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0,0,14)))); -// localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj"))); -// localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png"))); -// localBox.addComponent(new ComponentRenderTexturedStaticMesh( -// new Uri("DATA", "basic.vert"), -// new Uri("DATA", "basic.frag"))); -// env.addEntity(localBox); -// } - - Entity gird = new Entity(this.env); - gird.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0,0,13)))); - gird.addComponent(new ComponentStaticMesh(MeshGenerator.createGrid(5))); - gird.addComponent(new ComponentRenderColoredStaticMesh( - new Uri("DATA_EGE", "wireColor.vert"), - new Uri("DATA_EGE", "wireColor.frag"))); - env.addEntity(gird); - - Entity player = new Entity(this.env); - objectPosition = new ComponentPositionPlayer(new Transform3D(new Vector3f(5,5,13))); - player.addComponent(objectPosition); - objectPlayer = new ComponentPlayer(); - player.addComponent(objectPlayer); - player.addComponent(new ComponentMaterial(new Material())); - //player.addComponent(new ComponentStaticMesh(new Uri("RES", "person.obj"))); - player.addComponent(new ComponentStaticMesh(new Uri("RES", "person_-yfw_zup.obj"))); - player.addComponent(new ComponentTexture(new Uri("RES", "playerTexture.png"))); - player.addComponent(new ComponentRenderTexturedMaterialsStaticMesh( - new Uri("DATA", "basicMaterial.vert"), - new Uri("DATA", "basicMaterial.frag"), - (EngineLight)env.getEngine(EngineLight.ENGINE_NAME))); - ComponentPhysics physics = new ComponentPhysics(true); - PhysicBox box = new PhysicBox(); - box.setSize(new Vector3f(0.6f,0.6f,1.8f)); - box.setOrigin(new Vector3f(0,0,0.9f)); - box.setMass(1); - physics.addShape(box); - player.addComponent(physics); - env.addEntity(player); - - - Camera mainView = new Camera(); - env.addCamera("default", mainView); - mainView.setPitch((float)Math.PI*-0.25f); - mainView.setPosition(new Vector3f(0,-5,5)); - - this.simpleControl = new ControlCameraPlayer(mainView, player); - env.addControlInterface(simpleControl); - - // start the engine. - env.setPropertyStatus(GameStatus.gameStart); - - basicRotation.setEulerAngles(new Vector3f(0.005f,0.005f,0.01f)); - basicRotation2.setEulerAngles(new Vector3f(0.003f,0.01f,0.001f)); - // ready to let Gale & Ege manage the display - Log.info("==> Init APPL (END)"); - creationDone = true; } - */ - - public void onDraw(GaleContext context) { - if (this.debugDrawProperty == null) { + + public static void onDraw() { + if (debugDrawProperty == null) { debugDrawProperty = ResourceColored3DObject.create(); } // now render the point test collision ... - for (int iii=0; iii tmp = new ArrayList<>(); + tmp.add(new Vector3f(0, 0, 0)); + tmp.add(force); + debugDrawProperty.drawLine(tmp, new Color(1, 0, 0, 1), Matrix4f.createMatrixTranslate(subPosition), true, false); + } + public void setSize(Vector3f size) { this.size = size; } - + @Override - public void updateAABB(Transform3D transformGlobal, PhysicCollisionAABB aabb) { + public void updateAABB(Transform3D transformGlobal, AABB aabb) { // store it, many time usefull... this.transformGlobal = transformGlobal; this.colisionPoints.clear(); @@ -41,14 +102,7 @@ public class PhysicBox extends PhysicShape { aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(-this.size.x() * 0.5f, -this.size.y() * 0.5f, -this.size.z() * 0.5f)))); aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(this.size.x() * 0.5f, -this.size.y() * 0.5f, -this.size.z() * 0.5f)))); } - - // only needed for the narrow phase calculation ... - public Vector3f narrowPhaseGlobalPos; - public Vector3f narrowPhaseAxisX = new Vector3f(1, 0, 0); - public Vector3f narrowPhaseAxisY = new Vector3f(1, 0, 0); - public Vector3f narrowPhaseAxisZ = new Vector3f(1, 0, 0); - public Vector3f narrowPhaseHalfSize; - + @Override public void updateForNarrowCollision(Transform3D transformGlobal) { this.narrowPhaseGlobalPos = transformGlobal.multiply(this.transform.multiply(new Vector3f(0, 0, 0))); @@ -57,56 +111,5 @@ public class PhysicBox extends PhysicShape { this.narrowPhaseAxisZ = transformGlobal.multiply(this.transform.multiply(new Vector3f(0, 0, 1))).less(this.narrowPhaseGlobalPos); this.narrowPhaseHalfSize = this.size.multiply(0.5f); } - - private void renderPoint(Vector3f subPosition, Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) { - Matrix4f transformation = transformGlobal.getOpenGLMatrix().multiply(this.transform.getOpenGLMatrix()).multiply(Matrix4f.createMatrixTranslate(subPosition)); - - debugDrawProperty.drawSquare(new Vector3f(0.08f, 0.08f, 0.08f), transformation, new Color(0, 0, 1, 1)); - } - - private void renderPoint2(Vector3f subPosition, Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) { - Matrix4f transformation = transformGlobal.getOpenGLMatrix().multiply(this.transform.getOpenGLMatrix()).multiply(Matrix4f.createMatrixTranslate(subPosition)); - debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), transformation, new Color(0, 1, 0, 1)); - - } - - private void renderPoint3(Vector3f subPosition, Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) { - Matrix4f transformation = transformGlobal.getOpenGLMatrix().multiply(this.transform.getOpenGLMatrix()).multiply(Matrix4f.createMatrixTranslate(subPosition)); - debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), transformation, new Color(1, 1, 0, 1)); - - } - - private void renderPoint4(Vector3f subPosition, Vector3f force, ResourceColored3DObject debugDrawProperty) { - debugDrawProperty.drawSquare(new Vector3f(0.1f, 0.1f, 0.1f), Matrix4f.createMatrixTranslate(subPosition), new Color(1, 0, 0, 1)); - List tmp = new ArrayList<>(); - tmp.add(new Vector3f(0,0,0)); - tmp.add(force); - debugDrawProperty.drawLine(tmp, new Color(1, 0, 0, 1), Matrix4f.createMatrixTranslate(subPosition), true, false); - } - - @Override - public void renderDebug(Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) { - debugDrawProperty.drawSquare(this.size.multiply(0.5f), this.transform.getOpenGLMatrix().multiply(transformGlobal.getOpenGLMatrix()), new Color(0, 1, 0, 0.25f)); - Vector3f dimention = this.size.multiply(0.5f); - renderPoint2(new Vector3f(+dimention.x(), +dimention.y(), +dimention.z()), transformGlobal, debugDrawProperty); - renderPoint(new Vector3f(-dimention.x(), +dimention.y(), +dimention.z()), transformGlobal, debugDrawProperty); - renderPoint(new Vector3f(+dimention.x(), -dimention.y(), +dimention.z()), transformGlobal, debugDrawProperty); - renderPoint(new Vector3f(-dimention.x(), -dimention.y(), +dimention.z()), transformGlobal, debugDrawProperty); - renderPoint(new Vector3f(+dimention.x(), +dimention.y(), -dimention.z()), transformGlobal, debugDrawProperty); - renderPoint(new Vector3f(-dimention.x(), +dimention.y(), -dimention.z()), transformGlobal, debugDrawProperty); - renderPoint(new Vector3f(+dimention.x(), -dimention.y(), -dimention.z()), transformGlobal, debugDrawProperty); - renderPoint3(new Vector3f(-dimention.x(), -dimention.y(), -dimention.z()), transformGlobal, debugDrawProperty); - for (Collision elem: this.colisionPoints) { - if (elem != null) { - if (elem.colisionPointLocal == null) { - Log.error("colision point must be set !!!"); - continue; - } - for (int iii = 0; iii < elem.colisionPointLocal.length; iii++) { - renderPoint4(elem.colisionPointLocal[iii].position, elem.colisionPointLocal[iii].force, debugDrawProperty); - } - } - } - } - + } diff --git a/src/org/atriasoft/phyligram/PhysicCollisionAABB.java b/src/org/atriasoft/phyligram/PhysicCollisionAABB.java deleted file mode 100644 index 1348025..0000000 --- a/src/org/atriasoft/phyligram/PhysicCollisionAABB.java +++ /dev/null @@ -1,77 +0,0 @@ -package org.atriasoft.phyligram; - -import org.atriasoft.etk.math.Vector3f; - -public class PhysicCollisionAABB { - public float minX; - public float minY; - public float minZ; - public float maxX; - public float maxY; - public float maxZ; - public PhysicCollisionAABB(float minX, float minY, float minZ, float maxX, float maxY, float maxZ) { - super(); - this.minX = minX; - this.minY = minY; - this.minZ = minZ; - this.maxX = maxX; - this.maxY = maxY; - this.maxZ = maxZ; - } - public boolean intersect(PhysicCollisionAABB other) { - if (this == other) { - return false; - } - if (minX > other.maxX) { - return false; - } - if (maxX < other.minX) { - return false; - } - if (minY > other.maxY) { - return false; - } - if (maxY < other.minY) { - return false; - } - if (minZ > other.maxZ) { - return false; - } - if (maxZ < other.minZ) { - return false; - } - return true; - } - public void update(Vector3f point) { - if (minX > point.x()) { - minX = point.x(); - } - if (maxX < point.x()) { - maxX = point.x(); - } - if (minY > point.y()) { - minY = point.y(); - } - if (maxY < point.y()) { - maxY = point.y(); - } - if (minZ > point.z()) { - minZ = point.z(); - } - if (maxZ < point.z()) { - maxZ = point.z(); - } - } - public Vector3f getMin() { - return new Vector3f(minX, minY, minZ); - } - public Vector3f getMax() { - return new Vector3f(maxX, maxY, maxZ); - } - public static PhysicCollisionAABB beforeCalculated() { - // TODO Auto-generated method stub - return new PhysicCollisionAABB(999999,999999,999999,-999999,-999999,-999999); - } -} - - diff --git a/src/org/atriasoft/phyligram/PhysicHeightMapChunk.java b/src/org/atriasoft/phyligram/PhysicHeightMapChunk.java new file mode 100644 index 0000000..f8621c6 --- /dev/null +++ b/src/org/atriasoft/phyligram/PhysicHeightMapChunk.java @@ -0,0 +1,63 @@ +package org.atriasoft.phyligram; + +import org.atriasoft.ege.geometry.Triangle; +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; +import org.atriasoft.gale.resource.ResourceColored3DObject; +import org.atriasoft.phyligram.shape.AABB; + +public class PhysicHeightMapChunk extends PhysicShape { + // Box size property in X, Y and Z + private Triangle[] triangles = null; + public Triangle[] trianglesGlobalPos = null; + + public PhysicHeightMapChunk() { + + } + + public PhysicHeightMapChunk(Triangle[] triangles) { + this.triangles = triangles; + } + + public Triangle[] getTriangles() { + return this.triangles; + } + + @Override + public void renderDebug(Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) { + + } + + public void setTriangles(Triangle[] triangles) { + this.triangles = triangles; + this.trianglesGlobalPos = new Triangle[triangles.length]; + for (int iii = 0; iii < this.triangles.length; iii++) { + Vector3f data1 = this.transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p1)); + Vector3f data2 = this.transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p2)); + Vector3f data3 = this.transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p3)); + this.trianglesGlobalPos[iii] = new Triangle(data1, data2, data3); + } + } + + @Override + public void updateAABB(Transform3D transformGlobal, AABB aabb) { + // store it, many time usefull... + this.transformGlobal = transformGlobal; + this.colisionPoints.clear(); + for (int iii = 0; iii < this.triangles.length; iii++) { + aabb.update(transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p1))); + aabb.update(transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p2))); + aabb.update(transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p3))); + } + } + + @Override + public void updateForNarrowCollision(Transform3D transformGlobal) { + for (int iii = 0; iii < this.triangles.length; iii++) { + this.trianglesGlobalPos[iii].p1 = this.transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p1)); + this.trianglesGlobalPos[iii].p2 = this.transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p2)); + this.trianglesGlobalPos[iii].p3 = this.transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p3)); + } + } + +} diff --git a/src/org/atriasoft/phyligram/PhysicMapVoxel.java b/src/org/atriasoft/phyligram/PhysicMapVoxel.java index ec96f57..f13f9cb 100644 --- a/src/org/atriasoft/phyligram/PhysicMapVoxel.java +++ b/src/org/atriasoft/phyligram/PhysicMapVoxel.java @@ -1,42 +1,43 @@ package org.atriasoft.phyligram; +import org.atriasoft.ege.map.VoxelChunk; import org.atriasoft.etk.math.Transform3D; import org.atriasoft.etk.math.Vector3f; import org.atriasoft.gale.resource.ResourceColored3DObject; -import org.atriasoft.ege.map.VoxelChunk; +import org.atriasoft.phyligram.shape.AABB; public class PhysicMapVoxel extends PhysicShape { // Box size property in X, Y and Z private VoxelChunk chunk; + // only needed for the narrow phase calculation ... + private Vector3f narrowPhaseGlobalPos; + private Vector3f narrowPhaseAxisX = new Vector3f(1, 0, 0); + private Vector3f narrowPhaseAxisY = new Vector3f(1, 0, 0); + private Vector3f narrowPhaseAxisZ = new Vector3f(1, 0, 0); + private Vector3f narrowPhaseHalfSize = new Vector3f(0.5f, 0.5f, 0.5f); + public PhysicMapVoxel(VoxelChunk chunk) { - super(PhysicShapeType.MAP_VOXEL); this.chunk = chunk; } + @Override - public void updateAABB(Transform3D transform, PhysicCollisionAABB aabb) { + public void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty) { + + } + + @Override + public void updateAABB(Transform3D transform, AABB aabb) { if (this.chunk == null) { return; } this.colisionPoints.clear(); - aabb.update(new Vector3f(this.chunk.getPosition().x(),this.chunk.getPosition().y(),this.chunk.getPosition().z())); - aabb.update(new Vector3f( - this.chunk.getPosition().x() + VoxelChunk.VOXEL_CHUNK_SIZE, - this.chunk.getPosition().y() + VoxelChunk.VOXEL_CHUNK_SIZE, + aabb.update(new Vector3f(this.chunk.getPosition().x(), this.chunk.getPosition().y(), this.chunk.getPosition().z())); + aabb.update(new Vector3f(this.chunk.getPosition().x() + VoxelChunk.VOXEL_CHUNK_SIZE, this.chunk.getPosition().y() + VoxelChunk.VOXEL_CHUNK_SIZE, this.chunk.getPosition().z() + VoxelChunk.VOXEL_CHUNK_SIZE)); } - // only needed for the narrow phase calculation ... - private Vector3f narrowPhaseGlobalPos; - private Vector3f narrowPhaseAxisX = new Vector3f(1,0,0); - private Vector3f narrowPhaseAxisY = new Vector3f(1,0,0); - private Vector3f narrowPhaseAxisZ = new Vector3f(1,0,0); - private Vector3f narrowPhaseHalfSize = new Vector3f(0.5f,0.5f,0.5f); + @Override public void updateForNarrowCollision(Transform3D transform) { - this.narrowPhaseGlobalPos = transform.multiply(this.transform.multiply(new Vector3f(0,0,0))); - } - @Override - public void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty) { - - + this.narrowPhaseGlobalPos = transform.multiply(this.transform.multiply(new Vector3f(0, 0, 0))); } } diff --git a/src/org/atriasoft/phyligram/PhysicShape.java b/src/org/atriasoft/phyligram/PhysicShape.java index 3c90611..9999843 100644 --- a/src/org/atriasoft/phyligram/PhysicShape.java +++ b/src/org/atriasoft/phyligram/PhysicShape.java @@ -7,13 +7,9 @@ import org.atriasoft.etk.math.Quaternion; import org.atriasoft.etk.math.Transform3D; import org.atriasoft.etk.math.Vector3f; import org.atriasoft.gale.resource.ResourceColored3DObject; - - - +import org.atriasoft.phyligram.shape.AABB; public abstract class PhysicShape { - - protected List colisionPoints = new ArrayList<>(); // protected Quaternion quaternion; @@ -21,81 +17,73 @@ public abstract class PhysicShape { protected Transform3D transform; protected Transform3D transformGlobal; protected float mass = 0; - protected final PhysicShapeType type; - - public PhysicShape(PhysicShapeType type) { - this.type = type; + + public PhysicShape() { this.transform = Transform3D.IDENTITY; // this.quaternion = Quaternion.identity(); // this.origin = Vector3f.zero(); this.mass = 0; } - - public PhysicShape(PhysicShapeType type, Quaternion quaternion, Vector3f origin, float mass) { - this.type = type; + + public PhysicShape(Quaternion quaternion, Vector3f origin, float mass) { this.transform = new Transform3D(origin, quaternion); // this.quaternion = quaternion; // this.origin = origin; this.mass = mass; } - - public Quaternion getQuaternionFull() { - return transformGlobal.getOrientation().multiply(transform.getOrientation()); + + public void addColision(Collision colision) { + this.colisionPoints.add(colision); } - - public Quaternion getQuaternion() { - return transform.getOrientation(); + + public float getMass() { + return this.mass; } - - public void setQuaternion(Quaternion quaternion) { - this.transform = this.transform.withOrientation(quaternion); - } - + public Vector3f getOrigin() { return this.transform.getPosition(); } - - public void setOrigin(Vector3f origin) { - this.transform = this.transform.withPosition(origin); + + public Quaternion getQuaternion() { + return this.transform.getOrientation(); } - + + public Quaternion getQuaternionFull() { + return this.transformGlobal.getOrientation().multiply(this.transform.getOrientation()); + } + public Transform3D getTransform() { - return transform; + return this.transform; } - - public void setTransform(Transform3D transform) { - this.transform = transform; - } - + public Transform3D getTransformGlobal() { - return transformGlobal; + return this.transformGlobal; } - - public void setTransformGlobal(Transform3D transform) { - this.transformGlobal = transform; - } - - public float getMass() { - return mass; - } - + + public abstract void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty); + public void setMass(float mass) { this.mass = mass; } - - public PhysicShapeType getType() { - return type; - } - - public void addColision(Collision colision) { - colisionPoints.add(colision); + + public void setOrigin(Vector3f origin) { + this.transform = this.transform.withPosition(origin); } + public void setQuaternion(Quaternion quaternion) { + this.transform = this.transform.withOrientation(quaternion); + } + + public void setTransform(Transform3D transform) { + this.transform = transform; + } + + public void setTransformGlobal(Transform3D transform) { + this.transformGlobal = transform; + } + + public abstract void updateAABB(Transform3D transform, AABB aabb); - public abstract void updateAABB(Transform3D transform, PhysicCollisionAABB aabb); - public abstract void updateForNarrowCollision(Transform3D transform); - - public abstract void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty); - + } diff --git a/src/org/atriasoft/phyligram/PhysicShapeType.java b/src/org/atriasoft/phyligram/PhysicShapeType.java deleted file mode 100644 index 8f162cd..0000000 --- a/src/org/atriasoft/phyligram/PhysicShapeType.java +++ /dev/null @@ -1,13 +0,0 @@ -package org.atriasoft.phyligram; - -public enum PhysicShapeType { - UNKNOWN, - BOX, - CAPSULE, - CONE, - CONVEXHULL, - CYLINDER, - SPHERE, - CONCAVE, - MAP_VOXEL -} diff --git a/src/org/atriasoft/phyligram/PhysicSphere.java b/src/org/atriasoft/phyligram/PhysicSphere.java index 8a6b8af..00473fe 100644 --- a/src/org/atriasoft/phyligram/PhysicSphere.java +++ b/src/org/atriasoft/phyligram/PhysicSphere.java @@ -4,35 +4,46 @@ import org.atriasoft.etk.Color; import org.atriasoft.etk.math.Transform3D; import org.atriasoft.etk.math.Vector3f; import org.atriasoft.gale.resource.ResourceColored3DObject; +import org.atriasoft.phyligram.shape.AABB; public class PhysicSphere extends PhysicShape { // Box size property in X, Y and Z private float size; - public PhysicSphere() { - super(PhysicShapeType.SPHERE); - } + + // only needed for the narrow phase calculation ... + public Vector3f narrowPhaseGlobalPos; + + public PhysicSphere() {} + public float getSize() { - return size; + return this.size; } + + @Override + public void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty) { + debugDrawProperty.drawSphere(this.size, 9, 9, this.transform.getOpenGLMatrix().multiply(transform.getOpenGLMatrix()), new Color(0, 1, 0, 1)); + + } + public void setSize(float size) { this.size = size; } + @Override - public void updateAABB(Transform3D transform, PhysicCollisionAABB aabb) { - aabb.update(transform.multiply(this.transform.getPosition()).add(this.size,0,0)); - aabb.update(transform.multiply(this.transform.getPosition()).add(-this.size,0,0)); - aabb.update(transform.multiply(this.transform.getPosition()).add(0,this.size,0)); - aabb.update(transform.multiply(this.transform.getPosition()).add(0,-this.size,0)); - aabb.update(transform.multiply(this.transform.getPosition()).add(0,0,this.size)); - aabb.update(transform.multiply(this.transform.getPosition()).add(0,0,-this.size)); + public void updateAABB(Transform3D transformGlobal, AABB aabb) { + // store it, many time usefull... + this.transformGlobal = transformGlobal; + Vector3f basePosition = transformGlobal.multiply(this.transform.getPosition()); + aabb.update(basePosition.add(this.size, 0, 0)); + aabb.update(basePosition.add(-this.size, 0, 0)); + aabb.update(basePosition.add(0, this.size, 0)); + aabb.update(basePosition.add(0, -this.size, 0)); + aabb.update(basePosition.add(0, 0, this.size)); + aabb.update(basePosition.add(0, 0, -this.size)); } + @Override public void updateForNarrowCollision(Transform3D transform) { - - } - @Override - public void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty) { - debugDrawProperty.drawSphere(this.size, 9, 9, this.transform.getOpenGLMatrix().multiply(transform.getOpenGLMatrix()), new Color(0,1,0,1)); - + this.narrowPhaseGlobalPos = this.transformGlobal.multiply(this.transform.multiply(new Vector3f(0, 0, 0))); } } \ No newline at end of file diff --git a/src/org/atriasoft/phyligram/PhysicTriangle.java b/src/org/atriasoft/phyligram/PhysicTriangle.java new file mode 100644 index 0000000..5ab6e98 --- /dev/null +++ b/src/org/atriasoft/phyligram/PhysicTriangle.java @@ -0,0 +1,61 @@ +package org.atriasoft.phyligram; + +import org.atriasoft.ege.geometry.Triangle; +import org.atriasoft.etk.Color; +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; +import org.atriasoft.gale.resource.ResourceColored3DObject; +import org.atriasoft.phyligram.shape.AABB; + +public class PhysicTriangle extends PhysicShape { + // Box size property in X, Y and Z + private Triangle triangle; + + // only needed for the narrow phase calculation ... + public Triangle narrowPhaseGlobalTriangle; + + public PhysicTriangle() {} + + public Triangle getTriangle() { + return this.triangle; + } + + public Triangle getTriangleGlobalPos() { + return this.narrowPhaseGlobalTriangle; + } + + @Override + public void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty) { + debugDrawProperty.drawTriangle(this.triangle.p1, this.triangle.p2, this.triangle.p3, this.transform.getOpenGLMatrix().multiply(transform.getOpenGLMatrix()), new Color(0, 1, 0, 1)); + + } + + public void setPoints(Vector3f p1, Vector3f p2, Vector3f p3) { + this.triangle = new Triangle(p1, p2, p3); + } + + public void setTriangle(Triangle data) { + this.triangle = data.clone(); + } + + @Override + public void updateAABB(Transform3D transformGlobal, AABB aabb) { + // store it, many time usefull... + this.transformGlobal = transformGlobal; + Vector3f basePositionP1 = transformGlobal.multiply(this.transform.getPosition().add(this.triangle.p1)); + Vector3f basePositionP2 = transformGlobal.multiply(this.transform.getPosition().add(this.triangle.p2)); + Vector3f basePositionP3 = transformGlobal.multiply(this.transform.getPosition().add(this.triangle.p3)); + this.narrowPhaseGlobalTriangle = new Triangle(basePositionP1, basePositionP2, basePositionP3); + aabb.update(basePositionP1); + aabb.update(basePositionP2); + aabb.update(basePositionP3); + } + + @Override + public void updateForNarrowCollision(Transform3D transform) { + Vector3f basePositionP1 = this.transformGlobal.multiply(this.transform.multiply(this.triangle.p1)); + Vector3f basePositionP2 = this.transformGlobal.multiply(this.transform.multiply(this.triangle.p2)); + Vector3f basePositionP3 = this.transformGlobal.multiply(this.transform.multiply(this.triangle.p3)); + this.narrowPhaseGlobalTriangle = new Triangle(basePositionP1, basePositionP2, basePositionP3); + } +} \ No newline at end of file diff --git a/src/org/atriasoft/phyligram/ToolCollisionOBBWithOBB.java b/src/org/atriasoft/phyligram/ToolCollisionOBBWithOBB.java index e7286eb..f4262f2 100644 --- a/src/org/atriasoft/phyligram/ToolCollisionOBBWithOBB.java +++ b/src/org/atriasoft/phyligram/ToolCollisionOBBWithOBB.java @@ -141,7 +141,7 @@ public class ToolCollisionOBBWithOBB { // getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, rPos1); // fonctionne quand le block est trourner de 90% petit pb de positionnement en hauteur.... // getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, quat2.multiply(rPos2)); - ColisionPoints[] collide1 = getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, quat2.inverse().getMatrix4().multiply(rPos1)); + ColisionPoint[] collide1 = getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, quat2.inverse().getMatrix4().multiply(rPos1)); // transfer detection point collision in global environement: if (collide1 != null) { for (int iii = 0; iii < collide1.length; iii++) { @@ -155,7 +155,7 @@ public class ToolCollisionOBBWithOBB { DebugDisplay.testQTransfert = quatTransfer1; DebugDisplay.box1HalfSize = box1.narrowPhaseHalfSize; DebugDisplay.box2HalfSize = box2.narrowPhaseHalfSize; - ColisionPoints[] collide2 = getCollidePointsAABBCenteredWithOBB(box1.narrowPhaseHalfSize, box2.narrowPhaseHalfSize, quatTransfer1, quat1.inverse().getMatrix4().multiply(rPos2)); + ColisionPoint[] collide2 = getCollidePointsAABBCenteredWithOBB(box1.narrowPhaseHalfSize, box2.narrowPhaseHalfSize, quatTransfer1, quat1.inverse().getMatrix4().multiply(rPos2)); if (collide2 != null) { for (int iii = 0; iii < collide2.length; iii++) { collide2[iii].position = quat2.multiply(collide2[iii].position).add(box2.narrowPhaseGlobalPos); @@ -174,7 +174,7 @@ public class ToolCollisionOBBWithOBB { } - public static ColisionPoints[] getCollidePointsAABBCenteredWithOBB(Vector3f box1HalfSize, Vector3f box2HalfSize, Quaternion box2Orientation, Vector3f box2Position) { + public static ColisionPoint[] getCollidePointsAABBCenteredWithOBB(Vector3f box1HalfSize, Vector3f box2HalfSize, Quaternion box2Orientation, Vector3f box2Position) { // point in AABB Vector3f topBackRight = box2Orientation.multiply(new Vector3f(+box2HalfSize.x(), +box2HalfSize.y(), +box2HalfSize.z())).add(box2Position); @@ -245,38 +245,38 @@ public class ToolCollisionOBBWithOBB { if (insideBottomFrontLeft != null) { count++; } - ColisionPoints[] out = new ColisionPoints[count]; + ColisionPoint[] out = new ColisionPoint[count]; count = 0; if (insideTopBackRight != null) { - out[count] = new ColisionPoints(new Vector3f(+box2HalfSize.x(), +box2HalfSize.y(), +box2HalfSize.z()), insideTopBackRight); + out[count] = new ColisionPoint(new Vector3f(+box2HalfSize.x(), +box2HalfSize.y(), +box2HalfSize.z()), insideTopBackRight); count++; } if (insideTopBackLeft != null) { - out[count] = new ColisionPoints(new Vector3f(-box2HalfSize.x(), +box2HalfSize.y(), +box2HalfSize.z()), insideTopBackLeft); + out[count] = new ColisionPoint(new Vector3f(-box2HalfSize.x(), +box2HalfSize.y(), +box2HalfSize.z()), insideTopBackLeft); count++; } if (insideTopFrontRight != null) { - out[count] = new ColisionPoints(new Vector3f(+box2HalfSize.x(), -box2HalfSize.y(), +box2HalfSize.z()), insideTopFrontRight); + out[count] = new ColisionPoint(new Vector3f(+box2HalfSize.x(), -box2HalfSize.y(), +box2HalfSize.z()), insideTopFrontRight); count++; } if (insideTopFrontLeft != null) { - out[count] = new ColisionPoints(new Vector3f(-box2HalfSize.x(), -box2HalfSize.y(), +box2HalfSize.z()), insideTopFrontLeft); + out[count] = new ColisionPoint(new Vector3f(-box2HalfSize.x(), -box2HalfSize.y(), +box2HalfSize.z()), insideTopFrontLeft); count++; } if (insideBottomBackRight != null) { - out[count] = new ColisionPoints(new Vector3f(+box2HalfSize.x(), +box2HalfSize.y(), -box2HalfSize.z()), insideBottomBackRight); + out[count] = new ColisionPoint(new Vector3f(+box2HalfSize.x(), +box2HalfSize.y(), -box2HalfSize.z()), insideBottomBackRight); count++; } if (insideBottomBackLeft != null) { - out[count] = new ColisionPoints(new Vector3f(-box2HalfSize.x(), +box2HalfSize.y(), -box2HalfSize.z()), insideBottomBackLeft); + out[count] = new ColisionPoint(new Vector3f(-box2HalfSize.x(), +box2HalfSize.y(), -box2HalfSize.z()), insideBottomBackLeft); count++; } if (insideBottomFrontRight != null) { - out[count] = new ColisionPoints(new Vector3f(+box2HalfSize.x(), -box2HalfSize.y(), -box2HalfSize.z()), insideBottomFrontRight); + out[count] = new ColisionPoint(new Vector3f(+box2HalfSize.x(), -box2HalfSize.y(), -box2HalfSize.z()), insideBottomFrontRight); count++; } if (insideBottomFrontLeft != null) { - out[count] = new ColisionPoints(new Vector3f(-box2HalfSize.x(), -box2HalfSize.y(), -box2HalfSize.z()), insideBottomFrontLeft); + out[count] = new ColisionPoint(new Vector3f(-box2HalfSize.x(), -box2HalfSize.y(), -box2HalfSize.z()), insideBottomFrontLeft); count++; } if (count != 0) { diff --git a/src/org/atriasoft/phyligram/ToolCollisionSphereWithHeightMapChunk.java b/src/org/atriasoft/phyligram/ToolCollisionSphereWithHeightMapChunk.java new file mode 100644 index 0000000..e506061 --- /dev/null +++ b/src/org/atriasoft/phyligram/ToolCollisionSphereWithHeightMapChunk.java @@ -0,0 +1,63 @@ +package org.atriasoft.phyligram; + +import org.atriasoft.ege.geometry.Triangle; +import org.atriasoft.etk.math.Vector3f; + +public class ToolCollisionSphereWithHeightMapChunk { + //intersection entre 2 droite (en 2d avec estimation en 3D + public static Vector3f lineIntersect(Vector3f p1, Vector3f p2, Vector3f p3, Vector3f p4) { + float denom = (p4.y() - p3.y()) * (p2.x() - p1.x()) - (p4.x() - p3.x()) * (p2.y() - p1.y()); + if (denom == 0.0f) { // Lines are parallel. + return null; + } + float ua = ((p4.x() - p3.x()) * (p1.y() - p3.y()) - (p4.y() - p3.y()) * (p1.x() - p3.x())) / denom; + float ub = ((p2.x() - p1.x()) * (p1.y() - p3.y()) - (p2.y() - p1.y()) * (p1.x() - p3.x())) / denom; + if (ua >= 0.0f && ua <= 1.0f && ub >= 0.0f && ub <= 1.0f) { + // Get the intersection point. + return new Vector3f(p1.x() + ua * (p2.x() - p1.x()), p1.y() + ua * (p2.y() - p1.y()), p1.z() + ua * (p2.z() - p1.z())); + } + return null; + } + + public static Vector3f middlePoint(Vector3f p1, Vector3f p2, Vector3f p3) { + float distance1 = p1.distance(p2); + float distance2 = p1.distance(p3); + float ratio = distance1 / distance2; + return p3.add(p3.less(p1).multiply(ratio)); + } + + private static boolean pointInTriangle(Vector3f pt, Triangle triangle) { + float d1, d2, d3; + boolean has_neg, has_pos; + + d1 = sign(pt, triangle.p1, triangle.p2); + d2 = sign(pt, triangle.p2, triangle.p3); + d3 = sign(pt, triangle.p3, triangle.p1); + + has_neg = (d1 < 0) || (d2 < 0) || (d3 < 0); + has_pos = (d1 > 0) || (d2 > 0) || (d3 > 0); + + return !(has_neg && has_pos); + } + + private static float sign(Vector3f p1, Vector3f p2, Vector3f p3) { + return (p1.x() - p3.x()) * (p2.y() - p3.y()) - (p2.x() - p3.x()) * (p1.y() - p3.y()); + } + + public static boolean testCollide(PhysicSphere sphere1, PhysicHeightMapChunk map) { + for (Triangle elem : map.trianglesGlobalPos) { + if (pointInTriangle(sphere1.narrowPhaseGlobalPos, elem)) { + // calculate linear extrapolation of the z height at this position: + Vector3f position = lineIntersect(elem.p1, sphere1.narrowPhaseGlobalPos, elem.p2, elem.p3); + Vector3f middlePoint = middlePoint(elem.p1, sphere1.narrowPhaseGlobalPos, position); + float distanceSquare = middlePoint.distance2(sphere1.narrowPhaseGlobalPos); + if (sphere1.getSize() * sphere1.getSize() < distanceSquare) { + return true; + } + } + } + return false; + } + + private ToolCollisionSphereWithHeightMapChunk() {} +} diff --git a/src/org/atriasoft/phyligram/ToolCollisionSphereWithSphere.java b/src/org/atriasoft/phyligram/ToolCollisionSphereWithSphere.java new file mode 100644 index 0000000..584c825 --- /dev/null +++ b/src/org/atriasoft/phyligram/ToolCollisionSphereWithSphere.java @@ -0,0 +1,30 @@ +package org.atriasoft.phyligram; + +import org.atriasoft.etk.math.Vector3f; +import org.atriasoft.phyligram.internal.Log; + +public class ToolCollisionSphereWithSphere { + // Note sphere 2 is the reference ... + public static ColisionPoint getCollisionPoint(PhysicSphere sphere1, PhysicSphere shapeReference) { + if (sphere1.getSize() > shapeReference.getSize()) { + Log.todo("implement then reference is smaller than moving"); + } + Vector3f force = sphere1.narrowPhaseGlobalPos.less(shapeReference.narrowPhaseGlobalPos); + float distance = shapeReference.getSize() + sphere1.getSize() - force.length(); + force = force.safeNormalize(); + Vector3f impact = force.multiply(shapeReference.getSize()); + force = force.multiply(distance); + force = force.multiply(sphere1.getSize() + distance); + return new ColisionPoint(impact, force); + } + + // Note sphere 2 is the reference ... + public static boolean testCollide(PhysicSphere sphere1, PhysicSphere shapeReference) { + float distance1 = sphere1.narrowPhaseGlobalPos.distance2(shapeReference.narrowPhaseGlobalPos); + float distance2 = sphere1.getSize() * shapeReference.getSize(); + distance2 = distance2 * distance2; + return distance1 <= distance2; + } + + private ToolCollisionSphereWithSphere() {} +} diff --git a/src/org/atriasoft/phyligram/ToolCollisionSphereWithTriangle.java b/src/org/atriasoft/phyligram/ToolCollisionSphereWithTriangle.java new file mode 100644 index 0000000..87b34f1 --- /dev/null +++ b/src/org/atriasoft/phyligram/ToolCollisionSphereWithTriangle.java @@ -0,0 +1,155 @@ +package org.atriasoft.phyligram; + +import org.atriasoft.ege.geometry.Plane; +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Vector3f; + +// https://realtimecollisiondetection.net/blog/?p=103 +public class ToolCollisionSphereWithTriangle { + public static float clamp(float val, float min, float max) { + return Math.max(min, Math.min(max, val)); + } + + public static Vector3f getClosestPointOnFiniteLine(Vector3f point, Vector3f lineStart, Vector3f lineEnd) { + Vector3f lineDirection = lineEnd.less(lineStart); + float lineLength = lineDirection.length(); + lineDirection = lineDirection.normalize(); + float position = point.less(lineStart).dot(lineDirection); + float ProjectionLength = clamp(position, 0, lineLength); + return lineStart.add(lineDirection.multiply(ProjectionLength)); + } + + public static ColisionPoint getCollisionPoint(PhysicSphere sphere1, PhysicTriangle shapeReference) { + Plane plane = new Plane(shapeReference.getTriangleGlobalPos()); + float distance = plane.distance(sphere1.narrowPhaseGlobalPos); + float distance1 = distance * distance; + float distance2 = sphere1.getSize() * sphere1.getSize(); + if (distance1 > distance2) { + return null; + } + /* + if (distance < 0.0f) { + System.out.println("Distance : (BOTTOM) " + distance); + } else { + System.out.println("Distance : (TOP) " + distance); + } + */ + boolean inCenter = false; + // check the position with the perpendicular plan of each side of the triangle + Plane plane1 = new Plane(shapeReference.getTriangleGlobalPos().p1, shapeReference.getTriangleGlobalPos().p2, shapeReference.getTriangleGlobalPos().p1.add(plane.normal())); + float distanceBorder = plane1.distance(sphere1.narrowPhaseGlobalPos); + inCenter = distanceBorder < 0.0f; + if (inCenter == true) { + Plane plane2 = new Plane(shapeReference.getTriangleGlobalPos().p2, shapeReference.getTriangleGlobalPos().p3, shapeReference.getTriangleGlobalPos().p2.add(plane.normal())); + distanceBorder = plane2.distance(sphere1.narrowPhaseGlobalPos); + inCenter = distanceBorder < 0.0f; + } + if (inCenter == true) { + Plane plane3 = new Plane(shapeReference.getTriangleGlobalPos().p3, shapeReference.getTriangleGlobalPos().p1, shapeReference.getTriangleGlobalPos().p3.add(plane.normal())); + distanceBorder = plane3.distance(sphere1.narrowPhaseGlobalPos); + inCenter = distanceBorder < 0.0f; + } + if (inCenter == true) { + Vector3f impact = plane.minimalIntersectionPoint(sphere1.narrowPhaseGlobalPos); + Vector3f force = plane.normal(); + if (distance < 0) { + force = force.multiply(-(sphere1.getSize() + distance)); + } else { + force = force.multiply(-(sphere1.getSize() + distance)); + } + return new ColisionPoint(impact, force); + } + + System.out.println("Not in center"); + + // now we need to check if we have a collision with the border. + Vector3f nearestPointP1 = getClosestPointOnFiniteLine(sphere1.narrowPhaseGlobalPos, shapeReference.getTriangleGlobalPos().p1, shapeReference.getTriangleGlobalPos().p2); + float distanceP1Square = Vector3f.length2(nearestPointP1, sphere1.narrowPhaseGlobalPos); + System.out.println("distanceP1Square=" + distanceP1Square); + Vector3f nearestPointP2 = getClosestPointOnFiniteLine(sphere1.narrowPhaseGlobalPos, shapeReference.getTriangleGlobalPos().p2, shapeReference.getTriangleGlobalPos().p3); + float distanceP2Square = Vector3f.length2(nearestPointP2, sphere1.narrowPhaseGlobalPos); + System.out.println("distanceP2Square=" + distanceP2Square); + Vector3f nearestPointP3 = getClosestPointOnFiniteLine(sphere1.narrowPhaseGlobalPos, shapeReference.getTriangleGlobalPos().p3, shapeReference.getTriangleGlobalPos().p1); + float distanceP3Square = Vector3f.length2(nearestPointP3, sphere1.narrowPhaseGlobalPos); + System.out.println("distanceP3Square=" + distanceP3Square); + float distanceFinal; + Vector3f impact; + if (distanceP1Square < distanceP2Square && distanceP1Square < distanceP3Square) { + // P1 is the smallest + distanceFinal = FMath.sqrt(distanceP1Square); + impact = nearestPointP1; + } else if (distanceP2Square < distanceP3Square) { + // P2 is the smallest + distanceFinal = FMath.sqrt(distanceP2Square); + impact = nearestPointP2; + } else { + // P3 is the smallest + distanceFinal = FMath.sqrt(distanceP3Square); + impact = nearestPointP3; + } + + Vector3f force = sphere1.narrowPhaseGlobalPos.less(impact).safeNormalize(); + force = force.multiply(sphere1.getSize() - distanceFinal); + return new ColisionPoint(impact, force); + } + + public static boolean testCollide(PhysicSphere sphere1, PhysicTriangle shapeReference) { + Plane plane = new Plane(shapeReference.getTriangleGlobalPos()); + float distance = plane.distance(sphere1.narrowPhaseGlobalPos); + float distance1 = distance * distance; + float distance2 = sphere1.getSize() * sphere1.getSize(); + if (distance1 > distance2) { + return false; + } + /* + if (distance < 0.0f) { + System.out.println("Distance : (BOTTOM) " + distance); + } else { + System.out.println("Distance : (TOP) " + distance); + } + */ + boolean inCenter = false; + // check the position with the perpendicular plan of each side of the triangle + Plane plane1 = new Plane(shapeReference.getTriangleGlobalPos().p1, shapeReference.getTriangleGlobalPos().p2, shapeReference.getTriangleGlobalPos().p1.add(plane.normal())); + float distanceBorder = plane1.distance(sphere1.narrowPhaseGlobalPos); + inCenter = distanceBorder < 0.0f; + if (inCenter == true) { + Plane plane2 = new Plane(shapeReference.getTriangleGlobalPos().p2, shapeReference.getTriangleGlobalPos().p3, shapeReference.getTriangleGlobalPos().p2.add(plane.normal())); + distanceBorder = plane2.distance(sphere1.narrowPhaseGlobalPos); + inCenter = distanceBorder < 0.0f; + } + if (inCenter == true) { + Plane plane3 = new Plane(shapeReference.getTriangleGlobalPos().p3, shapeReference.getTriangleGlobalPos().p1, shapeReference.getTriangleGlobalPos().p3.add(plane.normal())); + distanceBorder = plane3.distance(sphere1.narrowPhaseGlobalPos); + inCenter = distanceBorder < 0.0f; + } + if (inCenter == true) { + return true; + } + + System.out.println("Not in center"); + + // now we need to check if we have a collision with the border. + Vector3f nearestPointP1 = getClosestPointOnFiniteLine(sphere1.narrowPhaseGlobalPos, shapeReference.getTriangleGlobalPos().p1, shapeReference.getTriangleGlobalPos().p2); + float distanceP1Square = Vector3f.length2(nearestPointP1, sphere1.narrowPhaseGlobalPos); + System.out.println("distanceP1Square=" + distanceP1Square); + if (distanceP1Square < distance2) { + return true; + } + Vector3f nearestPointP2 = getClosestPointOnFiniteLine(sphere1.narrowPhaseGlobalPos, shapeReference.getTriangleGlobalPos().p2, shapeReference.getTriangleGlobalPos().p3); + float distanceP2Square = Vector3f.length2(nearestPointP2, sphere1.narrowPhaseGlobalPos); + System.out.println("distanceP2Square=" + distanceP2Square); + if (distanceP2Square < distance2) { + return true; + } + Vector3f nearestPointP3 = getClosestPointOnFiniteLine(sphere1.narrowPhaseGlobalPos, shapeReference.getTriangleGlobalPos().p3, shapeReference.getTriangleGlobalPos().p1); + float distanceP3Square = Vector3f.length2(nearestPointP3, sphere1.narrowPhaseGlobalPos); + System.out.println("distanceP3Square=" + distanceP3Square); + if (distanceP3Square < distance2) { + return true; + } + return false; + } + + private ToolCollisionSphereWithTriangle() {} +} diff --git a/src/org/atriasoft/phyligram/math/Ray.java b/src/org/atriasoft/phyligram/math/Ray.java new file mode 100644 index 0000000..d904bed --- /dev/null +++ b/src/org/atriasoft/phyligram/math/Ray.java @@ -0,0 +1,53 @@ +package org.atriasoft.phyligram.math; + +import org.atriasoft.etk.math.Vector3f; + +public class Ray { + public float maxFraction; //!< Maximum fraction value + public Vector3f point1; //! aabb.minX) { + return false; + } + if (this.minY > aabb.minY) { + return false; + } + if (this.minZ > aabb.minZ) { + return false; + } + if (this.maxX < aabb.maxX) { + return false; + } + if (this.maxY < aabb.maxY) { + return false; + } + if (this.maxZ < aabb.maxZ) { + return false; + } + return true; + } + + public Vector3f getMax() { + return new Vector3f(this.maxX, this.maxY, this.maxZ); + } + + public Vector3f getMin() { + return new Vector3f(this.minX, this.minY, this.minZ); + } + + /** + * Get the cube total volume. + * @return + */ + public float getVolume() { + return ((this.maxX - this.minX) * (this.maxY - this.minY) * (this.maxZ - this.minZ)); + } + + public boolean intersect(AABB other) { + if (this == other) { + return false; + } + if (null == other) { + return false; + } + if (this.minX > other.maxX) { + return false; + } + if (this.maxX < other.minX) { + return false; + } + if (this.minY > other.maxY) { + return false; + } + if (this.maxY < other.minY) { + return false; + } + if (this.minZ > other.maxZ) { + return false; + } + if (this.maxZ < other.minZ) { + return false; + } + return true; + } + + public void lessMax(float deltaX, float deltaY, float deltaZ) { + this.maxX -= deltaX; + this.maxY -= deltaY; + this.maxZ -= deltaZ; + } + + public void lessMin(float deltaX, float deltaY, float deltaZ) { + this.minX -= deltaX; + this.minY -= deltaY; + this.minZ -= deltaZ; + } + + public void mergeTwoAABBs(AABB aaa, AABB bbb) { + this.minX = Math.min(aaa.minX, bbb.minX); + this.minY = Math.min(aaa.minY, bbb.minY); + this.minZ = Math.min(aaa.minZ, bbb.minZ); + this.minX = Math.max(aaa.maxX, bbb.maxX); + this.minY = Math.max(aaa.maxY, bbb.maxY); + this.minZ = Math.max(aaa.maxZ, bbb.maxZ); + } + + public void set(float minX, float minY, float minZ, float maxX, float maxY, float maxZ) { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + /* + * check if the ray intersects the AABB + * This method use the line vs AABB raycasting technique described in + * Real-time Collision Detection by Christer Ericson. + * @param ray Ray to test + * @return true The raytest intersect the AABB box + */ + public boolean testRayIntersect(final Ray ray) { + final Vector3f point2 = ray.point2.less(ray.point1).multiply(ray.maxFraction).add(ray.point1); + final float eeeX = this.maxX - this.minX; + final float eeeY = this.maxY - this.minY; + final float eeeZ = this.maxZ - this.minZ; + final Vector3f d = point2.less(ray.point1); + final float mmmX = ray.point1.x() - ray.point2.x() - this.minX - this.maxX; + final float mmmY = ray.point1.y() - ray.point2.y() - this.minY - this.maxY; + final float mmmZ = ray.point1.z() - ray.point2.z() - this.minZ - this.maxZ; + // Test if the AABB face normals are separating axis + float adx = FMath.abs(d.x()); + if (FMath.abs(mmmX) > eeeX + adx) { + return false; + } + float ady = FMath.abs(d.y()); + if (FMath.abs(mmmY) > eeeY + ady) { + return false; + } + float adz = FMath.abs(d.z()); + if (FMath.abs(mmmZ) > eeeZ + adz) { + return false; + } + // Add in an epsilon term to counteract arithmetic errors when segment is + // (near) parallel to a coordinate axis (see text for detail) + final float epsilon = 0.00001f; + adx += epsilon; + ady += epsilon; + adz += epsilon; + // Test if the cross products between face normals and ray direction are + // separating axis + if (FMath.abs(mmmY * d.z() - mmmZ * d.y()) > eeeY * adz + eeeZ * ady) { + return false; + } + if (FMath.abs(mmmZ * d.x() - mmmX * d.z()) > eeeX * adz + eeeZ * adx) { + return false; + } + if (FMath.abs(mmmX * d.y() - mmmY * d.x()) > eeeX * ady + eeeY * adx) { + return false; + } + // No separating axis has been found + return true; + } + + public void update(Vector3f point) { + if (this.minX > point.x()) { + this.minX = point.x(); + } + if (this.maxX < point.x()) { + this.maxX = point.x(); + } + if (this.minY > point.y()) { + this.minY = point.y(); + } + if (this.maxY < point.y()) { + this.maxY = point.y(); + } + if (this.minZ > point.z()) { + this.minZ = point.z(); + } + if (this.maxZ < point.z()) { + this.maxZ = point.z(); + } + } +} diff --git a/src/org/atriasoft/ege/physics/shape/Box.java b/src/org/atriasoft/phyligram/shape/Box.java similarity index 94% rename from src/org/atriasoft/ege/physics/shape/Box.java rename to src/org/atriasoft/phyligram/shape/Box.java index 625132f..29425fc 100644 --- a/src/org/atriasoft/ege/physics/shape/Box.java +++ b/src/org/atriasoft/phyligram/shape/Box.java @@ -3,7 +3,7 @@ * @copyright 2011, Edouard DUPIN, all right reserved * @license MPL v2.0 (see license file) */ -package org.atriasoft.ege.physics.shape; +package org.atriasoft.phyligram.shape; import org.atriasoft.etk.math.Vector3f; diff --git a/src/org/atriasoft/ege/physics/shape/Capsule.java b/src/org/atriasoft/phyligram/shape/Capsule.java similarity index 95% rename from src/org/atriasoft/ege/physics/shape/Capsule.java rename to src/org/atriasoft/phyligram/shape/Capsule.java index 401a7de..f213d45 100644 --- a/src/org/atriasoft/ege/physics/shape/Capsule.java +++ b/src/org/atriasoft/phyligram/shape/Capsule.java @@ -3,7 +3,7 @@ * @copyright 2011, Edouard DUPIN, all right reserved * @license MPL v2.0 (see license file) */ -package org.atriasoft.ege.physics.shape; +package org.atriasoft.phyligram.shape; import org.atriasoft.etk.math.Vector3f; diff --git a/src/org/atriasoft/ege/physics/shape/Concave.java b/src/org/atriasoft/phyligram/shape/Concave.java similarity index 96% rename from src/org/atriasoft/ege/physics/shape/Concave.java rename to src/org/atriasoft/phyligram/shape/Concave.java index fb79dab..1f1d365 100644 --- a/src/org/atriasoft/ege/physics/shape/Concave.java +++ b/src/org/atriasoft/phyligram/shape/Concave.java @@ -3,7 +3,7 @@ * @copyright 2011, Edouard DUPIN, all right reserved * @license MPL v2.0 (see license file) */ -package org.atriasoft.ege.physics.shape; +package org.atriasoft.phyligram.shape; import java.util.ArrayList; import java.util.List; diff --git a/src/org/atriasoft/ege/physics/shape/Cone.java b/src/org/atriasoft/phyligram/shape/Cone.java similarity index 95% rename from src/org/atriasoft/ege/physics/shape/Cone.java rename to src/org/atriasoft/phyligram/shape/Cone.java index 390ad53..66d8842 100644 --- a/src/org/atriasoft/ege/physics/shape/Cone.java +++ b/src/org/atriasoft/phyligram/shape/Cone.java @@ -3,7 +3,7 @@ * @copyright 2011, Edouard DUPIN, all right reserved * @license MPL v2.0 (see license file) */ -package org.atriasoft.ege.physics.shape; +package org.atriasoft.phyligram.shape; import org.atriasoft.etk.math.Vector3f; diff --git a/src/org/atriasoft/ege/physics/shape/ConvexHull.java b/src/org/atriasoft/phyligram/shape/ConvexHull.java similarity index 97% rename from src/org/atriasoft/ege/physics/shape/ConvexHull.java rename to src/org/atriasoft/phyligram/shape/ConvexHull.java index 8f884d7..f197567 100644 --- a/src/org/atriasoft/ege/physics/shape/ConvexHull.java +++ b/src/org/atriasoft/phyligram/shape/ConvexHull.java @@ -3,7 +3,7 @@ * @copyright 2011, Edouard DUPIN, all right reserved * @license MPL v2.0 (see license file) */ -package org.atriasoft.ege.physics.shape; +package org.atriasoft.phyligram.shape; import java.util.ArrayList; import java.util.List; diff --git a/src/org/atriasoft/ege/physics/shape/Cylinder.java b/src/org/atriasoft/phyligram/shape/Cylinder.java similarity index 94% rename from src/org/atriasoft/ege/physics/shape/Cylinder.java rename to src/org/atriasoft/phyligram/shape/Cylinder.java index acb169a..aae7ea4 100644 --- a/src/org/atriasoft/ege/physics/shape/Cylinder.java +++ b/src/org/atriasoft/phyligram/shape/Cylinder.java @@ -3,7 +3,7 @@ * @copyright 2011, Edouard DUPIN, all right reserved * @license MPL v2.0 (see license file) */ -package org.atriasoft.ege.physics.shape; +package org.atriasoft.phyligram.shape; import org.atriasoft.etk.math.Vector3f; diff --git a/src/org/atriasoft/phyligram/shape/HeightMapChunk.java b/src/org/atriasoft/phyligram/shape/HeightMapChunk.java new file mode 100644 index 0000000..80be6b4 --- /dev/null +++ b/src/org/atriasoft/phyligram/shape/HeightMapChunk.java @@ -0,0 +1,16 @@ +package org.atriasoft.phyligram.shape; + +import org.atriasoft.ephysics.collision.Triangle; + +public class HeightMapChunk extends Shape { + private Triangle[] triangles; + public HeightMapChunk(Triangle[] triangles) { + this.triangles = triangles; + } + @Override + public void display() { + // TODO Auto-generated method stub + super.display(); + } + +} diff --git a/src/org/atriasoft/ege/physics/shape/Shape.java b/src/org/atriasoft/phyligram/shape/Shape.java similarity index 84% rename from src/org/atriasoft/ege/physics/shape/Shape.java rename to src/org/atriasoft/phyligram/shape/Shape.java index 6f33506..a1280b0 100644 --- a/src/org/atriasoft/ege/physics/shape/Shape.java +++ b/src/org/atriasoft/phyligram/shape/Shape.java @@ -3,7 +3,7 @@ * @copyright 2011, Edouard DUPIN, all right reserved * @license MPL v2.0 (see license file) */ -package org.atriasoft.ege.physics.shape; +package org.atriasoft.phyligram.shape; import org.atriasoft.ege.internal.Log; import org.atriasoft.etk.math.Quaternion; @@ -30,34 +30,6 @@ public class Shape { return this.origin; } - public boolean isBox() { - return this instanceof Box; - }; - - public boolean isCapsule() { - return this instanceof Capsule; - } - - public boolean isConcave() { - return this instanceof Concave; - } - - public boolean isCone() { - return this instanceof Cone; - } - - public boolean isConvexHull() { - return this instanceof ConvexHull; - }; - - public boolean isCylinder() { - return this instanceof Cylinder; - }; - - public boolean isSphere() { - return this instanceof Sphere; - }; - public boolean parse(final String _line) { Log.error("dfgdfg"); /* diff --git a/src/org/atriasoft/ege/physics/shape/Sphere.java b/src/org/atriasoft/phyligram/shape/Sphere.java similarity index 94% rename from src/org/atriasoft/ege/physics/shape/Sphere.java rename to src/org/atriasoft/phyligram/shape/Sphere.java index a0d2bcd..c430300 100644 --- a/src/org/atriasoft/ege/physics/shape/Sphere.java +++ b/src/org/atriasoft/phyligram/shape/Sphere.java @@ -3,7 +3,7 @@ * @copyright 2011, Edouard DUPIN, all right reserved * @license MPL v2.0 (see license file) */ -package org.atriasoft.ege.physics.shape; +package org.atriasoft.phyligram.shape; import org.atriasoft.etk.math.Vector3f; diff --git a/src/org/atriasoft/phyligram/tree/CallbackOverlapping.java b/src/org/atriasoft/phyligram/tree/CallbackOverlapping.java new file mode 100644 index 0000000..04280f6 --- /dev/null +++ b/src/org/atriasoft/phyligram/tree/CallbackOverlapping.java @@ -0,0 +1,5 @@ +package org.atriasoft.phyligram.tree; + +public interface CallbackOverlapping { + public void callback(DTree nodeId); +} \ No newline at end of file diff --git a/src/org/atriasoft/phyligram/tree/CallbackRaycast.java b/src/org/atriasoft/phyligram/tree/CallbackRaycast.java new file mode 100644 index 0000000..72eedb2 --- /dev/null +++ b/src/org/atriasoft/phyligram/tree/CallbackRaycast.java @@ -0,0 +1,7 @@ +package org.atriasoft.phyligram.tree; + +import org.atriasoft.phyligram.math.Ray; + +public interface CallbackRaycast { + public float callback(DTree node, Ray ray); +} \ No newline at end of file diff --git a/src/org/atriasoft/phyligram/tree/DTree.java b/src/org/atriasoft/phyligram/tree/DTree.java new file mode 100644 index 0000000..d77f753 --- /dev/null +++ b/src/org/atriasoft/phyligram/tree/DTree.java @@ -0,0 +1,28 @@ +package org.atriasoft.phyligram.tree; + +import java.lang.ref.WeakReference; + +import org.atriasoft.phyligram.shape.AABB; + +/** + * It represents a node of the dynamic AABB tree. + */ +public class DTree { + private static int simpleCounter = 0; + public final int uid; + /** + * A node is either in the tree (has a parent) or in the free nodes list (has a next node) + */ + WeakReference parent = null; //!< Parent node ID (0 is ROOT) + int height = -1; //!< Height of the node in the tree TODO check the need... + AABB aabb = AABB.createInvertedEmpty(); //!< Fat axis aligned bounding box (AABB) corresponding to the node + + public DTree() { + this.uid = simpleCounter++; + } + + boolean isLeaf() { + return false; + } + +} diff --git a/src/org/atriasoft/phyligram/tree/DTreeLeafData.java b/src/org/atriasoft/phyligram/tree/DTreeLeafData.java new file mode 100644 index 0000000..1bf3096 --- /dev/null +++ b/src/org/atriasoft/phyligram/tree/DTreeLeafData.java @@ -0,0 +1,15 @@ +package org.atriasoft.phyligram.tree; + +class DTreeLeafData extends DTree { + Object dataPointer = null; + + public DTreeLeafData(final INTERNAL_DATA_TYPE dataPointer) { + super(); + this.dataPointer = dataPointer; + } + + @Override + boolean isLeaf() { + return true; + } +} \ No newline at end of file diff --git a/src/org/atriasoft/phyligram/tree/DTreeLeafInt.java b/src/org/atriasoft/phyligram/tree/DTreeLeafInt.java new file mode 100644 index 0000000..ab601f9 --- /dev/null +++ b/src/org/atriasoft/phyligram/tree/DTreeLeafInt.java @@ -0,0 +1,16 @@ +package org.atriasoft.phyligram.tree; + +class DTreeLeafInt extends DTree { + int dataInt0 = 0; + int dataInt1 = 0; + + public DTreeLeafInt(final int dataInt0, final int dataInt1) { + this.dataInt0 = dataInt0; + this.dataInt1 = dataInt1; + } + + @Override + boolean isLeaf() { + return true; + } +} diff --git a/src/org/atriasoft/phyligram/tree/DTreeNode.java b/src/org/atriasoft/phyligram/tree/DTreeNode.java new file mode 100644 index 0000000..ccbe43b --- /dev/null +++ b/src/org/atriasoft/phyligram/tree/DTreeNode.java @@ -0,0 +1,6 @@ +package org.atriasoft.phyligram.tree; + +class DTreeNode extends DTree { + DTree childrenleft = null; //!< Left child of the node + DTree childrenright = null; //!< Right child of the node +} \ No newline at end of file diff --git a/src/org/atriasoft/phyligram/tree/DynamicAABBTree.java b/src/org/atriasoft/phyligram/tree/DynamicAABBTree.java new file mode 100644 index 0000000..00beaf7 --- /dev/null +++ b/src/org/atriasoft/phyligram/tree/DynamicAABBTree.java @@ -0,0 +1,560 @@ +package org.atriasoft.phyligram.tree; + +import java.lang.ref.WeakReference; +import java.util.Stack; + +import org.atriasoft.phyligram.shape.AABB; +import org.atriasoft.phyligram.internal.Log; +import org.atriasoft.phyligram.math.Ray; +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Vector3f; + +/** + * It implements a dynamic AABB tree that is used for broad-phase + * collision detection. This data structure is inspired by Nathanael Presson's + * dynamic tree implementation in BulletPhysics. The following implementation is + * based on the one from Erin Catto in Box2D as described in the book + * "Introduction to Game Physics with Box2D" by Ian Parberry. + */ +public class DynamicAABBTree { + private DTree rootNode; //!< Pointer to the memory location of the nodes of the tree + + /// Constructor + public DynamicAABBTree() { + init(); + } + + /// Add an object into the tree (where node data is a pointer) + public DTree addObject(final AABB aabb, final INTERNAL_DATA_TYPE data) { + final DTreeLeafData node = new DTreeLeafData(data); + addObjectInternal(aabb, node); + return node; + } + + /// Internally add an object into the tree + private void addObjectInternal(final AABB aabb, final DTree leafNode) { + // Create the fat aabb to use in the tree + leafNode.aabb.clone(); + // Set the height of the node in the tree + leafNode.height = 0; + // Insert the new leaf node in the tree + insertLeafNode(leafNode); + assert (leafNode.isLeaf()); + } + + /// Balance the sub-tree of a given node using left or right rotations. + private DTree balanceSubTreeAtNode(final DTree node) { + assert (node != null); + // If the node is a leaf or the height of A's sub-tree is less than 2 + if (node.isLeaf() || node.height < 2) { + // Do not perform any rotation + return node; + } + final DTreeNode nodeA = (DTreeNode) node; + // Get the two children nodes + final DTree nodeB = nodeA.childrenleft; + final DTree nodeC = nodeA.childrenright; + // Compute the factor of the left and right sub-trees + final int balanceFactor = nodeC.height - nodeB.height; + // If the right node C is 2 higher than left node B + if (balanceFactor > 1) { + assert (!nodeC.isLeaf()); + final DTreeNode nodeCTree = (DTreeNode) nodeC; + final DTree nodeF = nodeCTree.childrenleft; + final DTree nodeG = nodeCTree.childrenright; + nodeCTree.childrenleft = node; + nodeCTree.parent = nodeA.parent; + nodeA.parent = new WeakReference<>(nodeC); + if (nodeC.parent != null) { + final DTreeNode nodeCParent = (DTreeNode) nodeC.parent.get(); + if (nodeCParent.childrenleft == node) { + nodeCParent.childrenleft = nodeC; + } else { + assert (nodeCParent.childrenright == node); + nodeCParent.childrenright = nodeC; + } + } else { + this.rootNode = nodeC; + } + assert (!nodeC.isLeaf()); + assert (!nodeA.isLeaf()); + // If the right node C was higher than left node B because of the F node + if (nodeF.height > nodeG.height) { + nodeCTree.childrenright = nodeF; + nodeA.childrenright = nodeG; + nodeG.parent = new WeakReference<>(node); + // Recompute the AABB of node A and C + nodeA.aabb.mergeTwoAABBs(nodeB.aabb, nodeG.aabb); + nodeC.aabb.mergeTwoAABBs(nodeA.aabb, nodeF.aabb); + // Recompute the height of node A and C + nodeA.height = FMath.max(nodeB.height, nodeG.height) + 1; + nodeC.height = FMath.max(nodeA.height, nodeF.height) + 1; + assert (nodeA.height > 0); + assert (nodeC.height > 0); + } else { + // If the right node C was higher than left node B because of node G + nodeCTree.childrenright = nodeG; + nodeA.childrenright = nodeF; + nodeF.parent = new WeakReference<>(node); + // Recompute the AABB of node A and C + nodeA.aabb.mergeTwoAABBs(nodeB.aabb, nodeF.aabb); + nodeC.aabb.mergeTwoAABBs(nodeA.aabb, nodeG.aabb); + // Recompute the height of node A and C + nodeA.height = FMath.max(nodeB.height, nodeF.height) + 1; + nodeC.height = FMath.max(nodeA.height, nodeG.height) + 1; + assert (nodeA.height > 0); + assert (nodeC.height > 0); + } + // Return the new root of the sub-tree + return nodeC; + } + // If the left node B is 2 higher than right node C + if (balanceFactor < -1) { + assert (!nodeB.isLeaf()); + final DTreeNode nodeBTree = (DTreeNode) nodeB; + final DTree nodeF = nodeBTree.childrenleft; + final DTree nodeG = nodeBTree.childrenright; + nodeBTree.childrenleft = node; + nodeB.parent = nodeA.parent; + nodeA.parent = new WeakReference<>(nodeB); + if (nodeB.parent != null) { + final DTreeNode nodeBParent = (DTreeNode) nodeB.parent.get(); + if (nodeBParent.childrenleft == node) { + nodeBParent.childrenleft = nodeB; + } else { + assert (nodeBParent.childrenright == node); + nodeBParent.childrenright = nodeB; + } + } else { + this.rootNode = nodeB; + } + assert (!nodeB.isLeaf()); + assert (!nodeA.isLeaf()); + // If the left node B was higher than right node C because of the F node + if (nodeF.height > nodeG.height) { + nodeBTree.childrenright = nodeF; + nodeA.childrenleft = nodeG; + nodeG.parent = new WeakReference<>(node); + // Recompute the AABB of node A and B + nodeA.aabb.mergeTwoAABBs(nodeC.aabb, nodeG.aabb); + nodeB.aabb.mergeTwoAABBs(nodeA.aabb, nodeF.aabb); + // Recompute the height of node A and B + nodeA.height = FMath.max(nodeC.height, nodeG.height) + 1; + nodeB.height = FMath.max(nodeA.height, nodeF.height) + 1; + assert (nodeA.height > 0); + assert (nodeB.height > 0); + } else { + // If the left node B was higher than right node C because of node G + nodeBTree.childrenright = nodeG; + nodeA.childrenleft = nodeF; + nodeF.parent = new WeakReference<>(node); + // Recompute the AABB of node A and B + nodeA.aabb.mergeTwoAABBs(nodeC.aabb, nodeF.aabb); + nodeB.aabb.mergeTwoAABBs(nodeA.aabb, nodeG.aabb); + // Recompute the height of node A and B + nodeA.height = FMath.max(nodeC.height, nodeF.height) + 1; + nodeB.height = FMath.max(nodeA.height, nodeG.height) + 1; + assert (nodeA.height > 0); + assert (nodeB.height > 0); + } + // Return the new root of the sub-tree + return nodeB; + } + // If the sub-tree is balanced, return the current root node + return node; + } + + /// Compute the height of the tree + public int computeHeight() { + return computeHeight(this.rootNode); + } + + /// Compute the height of a given node in the tree + private int computeHeight(final DTree node) { + // If the node is a leaf, its height is zero + if (node.isLeaf()) { + return 0; + } + final DTreeNode nodeTree = (DTreeNode) node; + + // Compute the height of the left and right sub-tree + final int leftHeight = computeHeight(nodeTree.childrenleft); + final int rightHeight = computeHeight(nodeTree.childrenright); + // Return the height of the node + return 1 + Math.max(leftHeight, rightHeight); + } + + /// Return the fat AABB corresponding to a given node ID + public AABB getFatAABB(final DTree node) { + return node.aabb; + } + + public int getNodeDataInt0(final DTree node) { + assert (node.isLeaf()); + return ((DTreeLeafInt) node).dataInt0; + } + + public int getNodeDataInt1(final DTree node) { + assert (node.isLeaf()); + return ((DTreeLeafInt) node).dataInt1; + } + + /// Return the data pointer of a given leaf node of the tree + public Object getNodeDataPointer(final DTree node) { + assert (node.isLeaf()); + return ((DTreeLeafData) node).dataPointer; + } + + /// Return the root AABB of the tree + public AABB getRootAABB() { + return getFatAABB(this.rootNode); + } + + /// Initialize the tree + private void init() { + this.rootNode = null; + } + + /// Insert a leaf node in the tree + private void insertLeafNode(final DTree inNode) { + // If the tree is empty + if (this.rootNode == null) { + this.rootNode = inNode; + return; + } + // Find the best sibling node for the new node + final AABB newNodeAABB = inNode.aabb; + DTree currentNode = this.rootNode; + while (!currentNode.isLeaf()) { + final DTreeNode node = (DTreeNode) currentNode; + final DTree leftChild = node.childrenleft; + final DTree rightChild = node.childrenright; + // Compute the merged AABB + final float volumeAABB = currentNode.aabb.getVolume(); + final AABB mergedAABBs = AABB.mergeAABB(currentNode.aabb, newNodeAABB); + final float mergedVolume = mergedAABBs.getVolume(); + // Compute the cost of making the current node the sibbling of the new node + final float costS = 2.0f * mergedVolume; + // Compute the minimum cost of pushing the new node further down the tree (inheritance cost) + final float costI = 2.0f * (mergedVolume - volumeAABB); + // Compute the cost of descending into the left child + float costLeft; + final AABB currentAndLeftAABB = new AABB(); + currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, leftChild.aabb); + if (leftChild.isLeaf()) { // If the left child is a leaf + costLeft = currentAndLeftAABB.getVolume() + costI; + } else { + final float leftChildVolume = leftChild.aabb.getVolume(); + costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume; + } + // Compute the cost of descending into the right child + float costRight; + final AABB currentAndRightAABB = new AABB(); + currentAndRightAABB.mergeTwoAABBs(newNodeAABB, rightChild.aabb); + if (rightChild.isLeaf()) { // If the right child is a leaf + costRight = currentAndRightAABB.getVolume() + costI; + } else { + final float rightChildVolume = rightChild.aabb.getVolume(); + costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume; + } + // If the cost of making the current node a sibbling of the new node is smaller than + // the cost of going down into the left or right child + if (costS < costLeft && costS < costRight) { + break; + } + // It is cheaper to go down into a child of the current node, choose the best child + if (costLeft < costRight) { + currentNode = leftChild; + } else { + currentNode = rightChild; + } + } + final DTree siblingNode = currentNode; + // Create a new parent for the new node and the sibling node + final WeakReference oldParentNode = siblingNode.parent; + final DTreeNode newParentNode = new DTreeNode(); + newParentNode.parent = currentNode.parent; + newParentNode.aabb.mergeTwoAABBs(currentNode.aabb, newNodeAABB); + newParentNode.height = currentNode.height + 1; + // If the sibling node was not the root node + if (oldParentNode != null) { + // replace in parent the child with the new child + final DTreeNode parentNode = (DTreeNode) oldParentNode.get(); + if (parentNode.childrenleft == siblingNode) { + parentNode.childrenleft = newParentNode; + } else { + parentNode.childrenright = newParentNode; + } + } else { + // If the sibling node was the root node + this.rootNode = newParentNode; + } + // setup the children + newParentNode.childrenleft = siblingNode; + newParentNode.childrenright = inNode; + siblingNode.parent = new WeakReference<>(newParentNode); + inNode.parent = new WeakReference<>(newParentNode); + + // Move up in the tree to change the AABBs that have changed + currentNode = newParentNode; + assert (!currentNode.isLeaf()); + while (currentNode != null) { + // Balance the sub-tree of the current node if it is not balanced + currentNode = balanceSubTreeAtNode(currentNode); + assert (inNode.isLeaf()); + assert (!currentNode.isLeaf()); + final DTreeNode nodeDouble = (DTreeNode) currentNode; + final DTree leftChild = nodeDouble.childrenleft; + final DTree rightChild = nodeDouble.childrenright; + assert (leftChild != null); + assert (rightChild != null); + // Recompute the height of the node in the tree + currentNode.height = FMath.max(leftChild.height, rightChild.height) + 1; + assert (currentNode.height > 0); + // Recompute the AABB of the node + currentNode.aabb.mergeTwoAABBs(leftChild.aabb, rightChild.aabb); + if (currentNode.parent != null) { + currentNode = currentNode.parent.get(); + } else { + currentNode = null; + } + } + assert (inNode.isLeaf()); + } + + /// Ray casting method + public void raycast(final Ray ray, final CallbackRaycast callback) { + if (callback == null) { + Log.error("call with null callback"); + return; + } + float maxFraction = ray.maxFraction; + // 128 max + final Stack stack = new Stack<>(); + stack.push(this.rootNode); + // Walk through the tree from the root looking for proxy shapes + // that overlap with the ray AABB + while (stack.size() > 0) { + // Get the next node in the stack + final DTree node = stack.pop(); + // If it is a null node, skip it + if (node == null) { + continue; + } + final Ray rayTemp = new Ray(ray.point1, ray.point2, maxFraction); + // Test if the ray intersects with the current node AABB + if (!node.aabb.testRayIntersect(rayTemp)) { + continue; + } + // If the node is a leaf of the tree + if (node.isLeaf()) { + // Call the callback that will raycast again the broad-phase shape + final float hitFraction = callback.callback(node, rayTemp); + // If the user returned a hitFraction of zero, it means that + // the raycasting should stop here + if (hitFraction == 0.0f) { + return; + } + // If the user returned a positive fraction + if (hitFraction > 0.0f) { + // We update the maxFraction value and the ray + // AABB using the new maximum fraction + if (hitFraction < maxFraction) { + maxFraction = hitFraction; + } + } + // If the user returned a negative fraction, we continue + // the raycasting as if the proxy shape did not exist + } else { // If the node has children + final DTreeNode tmpNode = (DTreeNode) node; + // Push its children in the stack of nodes to explore + stack.push(tmpNode.childrenleft); + stack.push(tmpNode.childrenright); + } + } + + } + + /// Release a node + private void releaseNode(final DTree node) { + //this.numberNodes--; + } + + /// Remove a leaf node from the tree + private void removeLeafNode(final DTree node) { + assert (node.isLeaf()); + // If we are removing the root node (root node is a leaf in this case) + if (this.rootNode == node) { + this.rootNode = null; + return; + } + // parent can not be null. + final DTreeNode parentNode = (DTreeNode) node.parent.get(); + final WeakReference grandParentNodeWeak = parentNode.parent; + DTree siblingNode; + if (parentNode.childrenleft == node) { + siblingNode = parentNode.childrenright; + } else { + siblingNode = parentNode.childrenleft; + } + // If the parent of the node to remove is not the root node + if (grandParentNodeWeak == null) { + // If the parent of the node to remove is the root node + // The sibling node becomes the new root node + this.rootNode = siblingNode; + siblingNode.parent = null; + releaseNode(parentNode); + } else { + final DTreeNode grandParentNode = (DTreeNode) grandParentNodeWeak.get(); + // Destroy the parent node + if (grandParentNode.childrenleft == parentNode) { + grandParentNode.childrenleft = siblingNode; + } else { + grandParentNode.childrenright = siblingNode; + } + siblingNode.parent = parentNode.parent; + releaseNode(parentNode); + // Now, we need to recompute the AABBs of the node on the path back to the root + // and make sure that the tree is still balanced + DTree currentNode = grandParentNode; + while (currentNode != null) { + // Balance the current sub-tree if necessary + currentNode = balanceSubTreeAtNode(currentNode); + assert (!currentNode.isLeaf()); + final DTreeNode currentTreeNode = (DTreeNode) currentNode; + // Get the two children of the current node + final DTree leftChild = currentTreeNode.childrenleft; + final DTree rightChild = currentTreeNode.childrenright; + // Recompute the AABB and the height of the current node + currentNode.aabb.mergeTwoAABBs(leftChild.aabb, rightChild.aabb); + currentNode.height = FMath.max(leftChild.height, rightChild.height) + 1; + assert (currentNode.height > 0); + if (currentNode.parent == null) { + currentNode = null; + } else { + currentNode = currentNode.parent.get(); + } + } + } + } + + /// Remove an object from the tree + public void removeObject(final DTree node) { + assert (node.isLeaf()); + // Remove the node from the tree + removeLeafNode(node); + releaseNode(node); + } + + /// Report all shapes overlapping with the AABB given in parameter. + public void reportAllShapesOverlappingWithAABB(final AABB aabb, final CallbackOverlapping callback) { + if (callback == null) { + Log.error("call with null callback"); + return; + } + //Log.error("reportAllShapesOverlappingWithAABB"); + // Create a stack with the nodes to visit + final Stack stack = new Stack<>(); + // 64 max + stack.push(this.rootNode); + //Log.error(" add stack: " + this.rootNode); + // While there are still nodes to visit + while (stack.size() > 0) { + // Get the next node ID to visit + final DTree nodeIDToVisit = stack.pop(); + // Skip it if it is a null node + if (nodeIDToVisit == null) { + continue; + } + // Get the corresponding node + final DTree nodeToVisit = nodeIDToVisit; + //Log.error(" check colision: " + nodeIDToVisit); + // If the AABB in parameter overlaps with the AABB of the node to visit + if (aabb.intersect(nodeToVisit.aabb)) { + // If the node is a leaf + if (nodeToVisit.isLeaf()) { + /* + if (aabb != nodeToVisit.aabb) { + Log.error(" ======> Real collision ..."); + } + */ + // Notify the broad-phase about a new potential overlapping pair + callback.callback(nodeIDToVisit); + } else { + final DTreeNode tmp = (DTreeNode) nodeToVisit; + // If the node is not a leaf + // We need to visit its children + stack.push(tmp.childrenleft); + stack.push(tmp.childrenright); + //Log.error(" add stack: " + tmp.childrenleft); + //Log.error(" add stack: " + tmp.childrenright); + } + } + } + } + + /// Clear all the nodes and reset the tree + public void reset() { + // Initialize the tree + init(); + } + + /// Update the dynamic tree after an object has moved. + /// If the new AABB of the object that has moved is still inside its fat AABB, then + /// nothing is done. Otherwise, the corresponding node is removed and reinserted into the tree. + /// The method returns true if the object has been reinserted into the tree. The "displacement" + /// argument is the linear velocity of the AABB multiplied by the elapsed time between two + /// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node + /// (this can be useful if the shape AABB has become much smaller than the previous one for instance). + public boolean updateObject(final DTree node, final AABB newAABB, final Vector3f displacement) { + return updateObject(node, newAABB, displacement, false); + } + + public boolean updateObject(final DTree node, final AABB newAABB, final Vector3f displacement, final boolean forceReinsert) { + assert (node.isLeaf()); + assert (node.height >= 0); + //Log.verbose(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax()); + //Log.verbose(" : " + newAABB.getMin() + " " + newAABB.getMax()); + // If the new AABB is still inside the fat AABB of the node + if (!forceReinsert && node.aabb.contains(newAABB)) { + return false; + } + // If the new AABB is outside the fat AABB, we remove the corresponding node + removeLeafNode(node); + // Compute the fat AABB by inflating the AABB with a ant gap + node.aabb = newAABB.clone(); + // Inflate the fat AABB in direction of the linear motion of the AABB + float xmin = node.aabb.getMin().x(); + float ymin = node.aabb.getMin().y(); + float zmin = node.aabb.getMin().z(); + float xmax = node.aabb.getMax().x(); + float ymax = node.aabb.getMax().y(); + float zmax = node.aabb.getMax().z(); + if (displacement.x() < 0.0f) { + xmin = node.aabb.getMin().x() + displacement.x(); + } else { + xmax = node.aabb.getMax().x() + displacement.x(); + } + if (displacement.y() < 0.0f) { + ymin = node.aabb.getMin().y() + displacement.y(); + } else { + ymax = node.aabb.getMax().y() + displacement.y(); + } + if (displacement.z() < 0.0f) { + zmin = node.aabb.getMin().z() + displacement.z(); + } else { + zmax = node.aabb.getMax().z() + displacement.z(); + } + node.aabb.set(xmin, ymin, zmin, xmax, ymax, zmax); + //Log.error(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax()); + //Log.error(" : " + newAABB.getMin() + " " + newAABB.getMax()); + if (!node.aabb.contains(newAABB)) { + //Log.critical("ERROR"); + } + assert (node.aabb.contains(newAABB)); + // Reinsert the node into the tree + insertLeafNode(node); + return true; + } + +} diff --git a/src/org/atriasoft/phyligram/tree/PairDTree.java b/src/org/atriasoft/phyligram/tree/PairDTree.java new file mode 100644 index 0000000..ff78cb0 --- /dev/null +++ b/src/org/atriasoft/phyligram/tree/PairDTree.java @@ -0,0 +1,37 @@ +package org.atriasoft.phyligram.tree; + +import java.util.Set; + +public record PairDTree( + DTree first, + DTree second) { + public static int countInSet(final Set values, final PairDTree sample) { + int count = 0; + for (final PairDTree elem : values) { + if (elem.first != sample.first) { + continue; + } + if (elem.second != sample.second) { + continue; + } + count++; + } + return count; + } + + public PairDTree(final DTree first, final DTree second) { + if (first.uid < second.uid) { + this.first = first; + this.second = second; + } else { + this.first = second; + this.second = first; + + } + } + + @Override + public String toString() { + return "PairDTree [first=" + this.first.uid + ", second=" + this.second.uid + "]"; + } +} diff --git a/test/src/test/atriasoft/ege/TestTransformation3D.java b/test/src/test/atriasoft/ege/TestTransformation3D.java index ee79095..5a4841c 100644 --- a/test/src/test/atriasoft/ege/TestTransformation3D.java +++ b/test/src/test/atriasoft/ege/TestTransformation3D.java @@ -3,7 +3,7 @@ package test.atriasoft.ege; import org.atriasoft.ege.geometry.AABB; import org.atriasoft.ege.geometry.Geometry3D; import org.atriasoft.ege.geometry.OBB; -import org.atriasoft.ege.geometry.Plane; +import org.atriasoft.ege.geometry.Plane____; import org.atriasoft.ege.geometry.Sphere; import org.atriasoft.ege.geometry.Triangle; import org.atriasoft.etk.math.Matrix3f; @@ -53,7 +53,7 @@ public class TestTransformation3D { @Test void testPointInPlane() { - final Plane shape = new Plane((new Vector3f(4, 4, 4)).normalize(), (float) Math.sqrt(1 * 1 + 1 * 1)); + final Plane____ shape = new Plane____((new Vector3f(4, 4, 4)).normalize(), (float) Math.sqrt(1 * 1 + 1 * 1)); Assert.assertFalse(Geometry3D.pointInPlane(new Vector3f(0, 0, 0), shape)); Assert.assertFalse(Geometry3D.pointInPlane(new Vector3f(6, 6, 6), shape)); Assert.assertTrue(Geometry3D.pointInPlane(new Vector3f(3, 3, 3), shape)); diff --git a/test/src/test/atriasoft/phyligram/TestCollisionSphereTriangle.java b/test/src/test/atriasoft/phyligram/TestCollisionSphereTriangle.java new file mode 100644 index 0000000..4d74a04 --- /dev/null +++ b/test/src/test/atriasoft/phyligram/TestCollisionSphereTriangle.java @@ -0,0 +1,77 @@ +package test.atriasoft.phyligram; + +import java.util.stream.Stream; + +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; +import org.atriasoft.phyligram.PhysicSphere; +import org.atriasoft.phyligram.PhysicTriangle; +import org.atriasoft.phyligram.ToolCollisionSphereWithTriangle; +import org.atriasoft.phyligram.shape.AABB; +import org.junit.Assert; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.Arguments; +import org.junit.jupiter.params.provider.MethodSource; + +public class TestCollisionSphereTriangle { + + static Stream generateData() { + return Stream.of( // ... + Arguments.of(1, new Vector3f(0.0f, 0.0f, 1.001f), false), // test just on top Z + Arguments.of(2, new Vector3f(0.0f, 0.0f, -1.001f), false), // test just on bottom Z + Arguments.of(2, new Vector3f(2.001f, 0.0f, 0.0f), false), // test on the top X + Arguments.of(2, new Vector3f(-2.001f, 0.0f, 0.0f), false), // test on the bottom X + Arguments.of(2, new Vector3f(0.0f, 1.7f, 0.0f), false), // test on the top Y + Arguments.of(2, new Vector3f(0.0f, -1.7f, 0.0f), false), // test on the bottom Y + // test collision + Arguments.of(2, new Vector3f(0.0f, 0.0f, 0.9f), true), // in center + Arguments.of(1, new Vector3f(0.0f, 0.0f, 0.999f), true), // test just on top Z + Arguments.of(2, new Vector3f(0.0f, 0.0f, -0.999f), true), // test just on bottom Z + Arguments.of(2, new Vector3f(1.999f, 0.0f, 0.0f), true), // test on the top X + Arguments.of(2, new Vector3f(-1.999f, 0.0f, 0.0f), true), // test on the bottom X + Arguments.of(2, new Vector3f(0.0f, 1.5f, 0.0f), true), // test on the top Y + Arguments.of(2, new Vector3f(0.0f, -1.5f, 0.0f), true), // test on the bottom Y + Arguments.of(2, new Vector3f(1.5f, 1.5f, 0.0f), true), // test on corner A + Arguments.of(2, new Vector3f(1.5f, -1.5f, 0.0f), true) // test on corner B + ); + } + + @ParameterizedTest + @MethodSource("generateData") + void testsphereOut(int testId, Vector3f position, boolean resultTheoricValue) { + System.out.println("AAAAA "); + float testCoefficient = 1.0f; + PhysicSphere sphere = new PhysicSphere(); + sphere.setSize(testCoefficient); + PhysicTriangle triangle = new PhysicTriangle(); + triangle.setPoints(new Vector3f(testCoefficient, testCoefficient, 0.0f), new Vector3f(testCoefficient, -testCoefficient, 0.0f), new Vector3f(-testCoefficient, 0.0f, 0.0f)); + Transform3D transformGlobalTriangle = Transform3D.IDENTITY; + Transform3D transformGlobalsphere = new Transform3D(position.multiply(testCoefficient)); + AABB aabb = new AABB(); + sphere.updateAABB(transformGlobalsphere, aabb); + sphere.updateForNarrowCollision(transformGlobalsphere); + triangle.updateAABB(transformGlobalTriangle, aabb); + triangle.updateForNarrowCollision(transformGlobalTriangle); + boolean result = ToolCollisionSphereWithTriangle.testCollide(sphere, triangle); + Assert.assertEquals(resultTheoricValue, result); + } + + @Test + void testsphereOutTop() { + float testCoefficient = 2.0f; + PhysicSphere sphere = new PhysicSphere(); + sphere.setSize(testCoefficient); + PhysicTriangle triangle = new PhysicTriangle(); + triangle.setPoints(new Vector3f(testCoefficient, testCoefficient, 0.0f), new Vector3f(testCoefficient, -testCoefficient, 0.0f), new Vector3f(-testCoefficient, 0.0f, 0.0f)); + Transform3D transformGlobalTriangle = Transform3D.IDENTITY; + Transform3D transformGlobalsphere = new Transform3D(new Vector3f(0.0f, 0.0f, testCoefficient + 0.001f)); + AABB aabb = new AABB(); + sphere.updateAABB(transformGlobalsphere, aabb); + sphere.updateForNarrowCollision(transformGlobalsphere); + triangle.updateAABB(transformGlobalTriangle, aabb); + triangle.updateForNarrowCollision(transformGlobalTriangle); + boolean result = ToolCollisionSphereWithTriangle.testCollide(sphere, triangle); + Assert.assertFalse(result); + } +}