From 98f5d7a1eda8fc9cdeaf285ad751b30d461621f6 Mon Sep 17 00:00:00 2001 From: ryanhcode <42245712+ryanhcode@users.noreply.github.co> Date: Fri, 1 Apr 2022 15:34:30 -0400 Subject: [PATCH] Integrate testing collision system, fix server-client sync issues, add collision quality options in config --- .../AirshipContraptionEntity.java | 45 +- .../createaeronautics/index/CAConfig.java | 9 + .../createaeronautics/index/CADimensions.java | 3 +- .../physics/AbstractContraptionRigidbody.java | 21 + .../createaeronautics/physics/IRigidbody.java | 4 +- .../SimulatedContraptionRigidbody.java | 171 +++-- .../physics/SubcontraptionRigidbody.java | 18 +- .../physics/api/ICustomCollisionShape.java | 26 + .../physics/api/IHotAirProvider.java | 13 + .../physics/api/IMassProvider.java | 13 + .../physics/api/IThrustProvider.java | 10 + ...ractContraptionBasedCollisionDetector.java | 84 +++ .../physics/collision/detection/Contact.java | 46 ++ .../collision/detection/EvolutionResult.java | 16 + .../physics/collision/detection/GJKEPA.java | 600 ++++++++++++++++++ .../detection/ICollisionDetector.java | 22 + .../physics/collision/detection/Manifold.java | 20 + .../physics/collision/detection/Simplex.java | 20 + .../physics/collision/detection/Support.java | 66 ++ .../physics/collision/detection/Triangle.java | 25 + .../detection/impl/GJKCollisionDetector.java | 21 + .../impl/SphereAABBCollisionDetector.java | 136 ++++ .../impl/SphereCollisionDetector.java | 90 +++ .../resolution/IIterativeManifoldSolver.java | 34 + .../resolution/SequentialManifoldSolver.java | 105 +++ .../collision/shape/CollisionQuality.java | 35 + .../collision/shape/ICollisionShape.java | 45 ++ .../shape/ICollisionShapeGenerator.java | 30 + .../collision/shape/MeshCollisionShape.java | 83 +++ .../shape/MeshCollisionShapeGenerator.java | 94 +++ 30 files changed, 1833 insertions(+), 72 deletions(-) create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/api/ICustomCollisionShape.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/api/IHotAirProvider.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/api/IMassProvider.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/api/IThrustProvider.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/AbstractContraptionBasedCollisionDetector.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Contact.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/EvolutionResult.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/GJKEPA.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/ICollisionDetector.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Manifold.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Simplex.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Support.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Triangle.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/impl/GJKCollisionDetector.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/impl/SphereAABBCollisionDetector.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/impl/SphereCollisionDetector.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/resolution/IIterativeManifoldSolver.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/resolution/SequentialManifoldSolver.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/CollisionQuality.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/ICollisionShape.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/ICollisionShapeGenerator.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/MeshCollisionShape.java create mode 100644 src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/MeshCollisionShapeGenerator.java diff --git a/src/main/java/com/eriksonn/createaeronautics/contraptions/AirshipContraptionEntity.java b/src/main/java/com/eriksonn/createaeronautics/contraptions/AirshipContraptionEntity.java index fa48f54..063cc17 100644 --- a/src/main/java/com/eriksonn/createaeronautics/contraptions/AirshipContraptionEntity.java +++ b/src/main/java/com/eriksonn/createaeronautics/contraptions/AirshipContraptionEntity.java @@ -9,6 +9,8 @@ import com.eriksonn.createaeronautics.network.packet.*; import com.eriksonn.createaeronautics.physics.SimulatedContraptionRigidbody; import com.eriksonn.createaeronautics.physics.SubcontraptionRigidbody; +import com.eriksonn.createaeronautics.physics.collision.shape.ICollisionShape; +import com.eriksonn.createaeronautics.physics.collision.shape.MeshCollisionShape; import com.eriksonn.createaeronautics.utils.AbstractContraptionEntityExtension; import com.eriksonn.createaeronautics.utils.Matrix3dExtension; import com.eriksonn.createaeronautics.world.FakeAirshipClientWorld; @@ -87,6 +89,10 @@ public AirshipContraptionEntity(EntityType type, World world) { super(type, world); sails = new HashMap<>(); simulatedRigidbody = new SimulatedContraptionRigidbody(this); + + // testing +// this.simulatedRigidbody.angularMomentum = new Vector3d(0, 0, 40); + System.out.println("New airship entity"); } @@ -126,12 +132,11 @@ public void tickContraption() { if (level.isClientSide) { profiler.startTick(); fakeClientWorld.tick(() -> true); - fakeClientWorld.tickEntities(); - for (ControlledContraptionEntity contraptionEntity : subContraptions.values()) { contraptionEntity.tick(); } + fakeClientWorld.tickBlockEntities(); profiler.endTick(); if (invalid) { @@ -162,28 +167,17 @@ public void tickContraption() { //for(Map.Entry entry : simulatedRigidbody.subcontraptionRigidbodyMap.entrySet()) //{ // SubcontraptionRigidbody rigidbody = entry.getValue(); - // Vector3d particlePos = rigidbody.toGlobal(new Vector3d(0,0,0)); - // level.addParticle(new RedstoneParticleData(1,1,1,1),particlePos.x,particlePos.y,particlePos.z,0,0,0); - //} +// x - if(level.isClientSide && false) { - CompoundNBT tag = this.entityData.get(physicsDataAccessor); - if(tag.contains("velocity")) { - simulatedRigidbody.globalVelocity = simulatedRigidbody.arrayToVec(readDoubleArray(tag, "velocity")); - //simulatedRigidbody.angularVelocity = simulatedRigidbody.arrayToVec(readDoubleArray(tag, "angularVelocity")); - simulatedRigidbody.angularMomentum = simulatedRigidbody.arrayToVec(readDoubleArray(tag, "angularMomentum")); - //simulatedRigidbody.momentum = simulatedRigidbody.arrayToVec(readDoubleArray(tag, "momentum")); - simulatedRigidbody.orientation = simulatedRigidbody.arrayToQuat(readDoubleArray(tag, "orientation")); - } - } + + //} // if(level.isClientSide) - //Vector3d particlePos = toGlobalVector(new Vector3d(0,0,0),0); - //level.addParticle(new RedstoneParticleData(1,1,1,1),particlePos.x,particlePos.y,particlePos.z,0,0,0); - //this.getContraption().getContraptionWorld().tickBlockEntities(); +// Vector3d particlePos = simulatedRigidbody.toGlobal(simulatedRigidbody.toLocal(simulatedRigidbody.toGlobal(new Vector3d(1,1,1)))); +// level.addParticle(new RedstoneParticleData(1,1,1,1),particlePos.x,particlePos.y,particlePos.z,0,0,0); } @@ -212,17 +206,20 @@ private double[] readDoubleArray(CompoundNBT tag, String key) { @Override public void onSyncedDataUpdated(DataParameter pKey) { - if (pKey == physicsDataAccessor) { + if (pKey.equals(physicsDataAccessor)) { CompoundNBT tag = this.entityData.get((DataParameter) pKey); - + simulatedRigidbody.momentum = simulatedRigidbody.arrayToVec(readDoubleArray(tag, "momentum")); + simulatedRigidbody.angularMomentum = simulatedRigidbody.arrayToVec(readDoubleArray(tag, "angularMomentum")); + simulatedRigidbody.orientation = simulatedRigidbody.arrayToQuat(readDoubleArray(tag, "orientation")); } } @Override public void onRemovedFromWorld() { subContraptions.forEach((uuid, contraptionEntity) -> { - contraptionEntity.disassemble(); + contraptionEntity.getContraption().addBlocksToWorld(contraptionEntity.level, ((ControlledContraptionEntityMixin)contraptionEntity).invokeMakeStructureTransform()); + contraptionEntity.remove(); serverDestroySubContraption(contraptionEntity); }); super.onRemovedFromWorld(); @@ -246,10 +243,8 @@ public void serverUpdate() { } CompoundNBT tag = new CompoundNBT(); - putDoubleArray(tag, "velocity", simulatedRigidbody.vecToArray(simulatedRigidbody.globalVelocity)); - //putDoubleArray(tag, "angularVelocity", simulatedRigidbody.vecToArray(simulatedRigidbody.angularVelocity)); - //putDoubleArray(tag, "angularMomentum", simulatedRigidbody.vecToArray(simulatedRigidbody.angularVelocity)); - //putDoubleArray(tag, "momentum", simulatedRigidbody.vecToArray(simulatedRigidbody.angularVelocity)); + putDoubleArray(tag, "angularMomentum", simulatedRigidbody.vecToArray(simulatedRigidbody.angularMomentum)); + putDoubleArray(tag, "momentum", simulatedRigidbody.vecToArray(simulatedRigidbody.momentum)); putDoubleArray(tag, "orientation", simulatedRigidbody.quatToArray(simulatedRigidbody.orientation)); this.entityData.set(physicsDataAccessor, tag); diff --git a/src/main/java/com/eriksonn/createaeronautics/index/CAConfig.java b/src/main/java/com/eriksonn/createaeronautics/index/CAConfig.java index f223fcb..411a43e 100644 --- a/src/main/java/com/eriksonn/createaeronautics/index/CAConfig.java +++ b/src/main/java/com/eriksonn/createaeronautics/index/CAConfig.java @@ -1,6 +1,7 @@ package com.eriksonn.createaeronautics.index; import com.eriksonn.createaeronautics.CreateAeronautics; +import com.eriksonn.createaeronautics.physics.collision.shape.CollisionQuality; import net.minecraftforge.common.ForgeConfigSpec; /** @@ -12,6 +13,8 @@ public class CAConfig { public static final String SERVER_FILENAME = CreateAeronautics.MODID + "-server.toml"; public static final String PHYSICS_CATEGORY = "Physics"; + public static final ForgeConfigSpec.EnumValue COLLISION_QUALITY; + public static final ForgeConfigSpec.IntValue MAX_COLLISION_ITERATIONS; public static final ForgeConfigSpec.DoubleValue LIGHT_BLOCK_WEIGHT; public static final ForgeConfigSpec.DoubleValue DEFAULT_BLOCK_WEIGHT; @@ -19,6 +22,12 @@ public class CAConfig { ForgeConfigSpec.Builder builder = new ForgeConfigSpec.Builder(); builder.push(PHYSICS_CATEGORY); + COLLISION_QUALITY = builder + .comment("What quality of collision detection should simulated contraptions use?") + .defineEnum("collisionQuality", CollisionQuality.GJKEPA); + MAX_COLLISION_ITERATIONS = builder + .comment("How many iterations should the collision engine be allowed run per tick?") + .defineInRange("maxIterations", 3, 1, 20); LIGHT_BLOCK_WEIGHT = builder .comment("Some blocks have the 'light' tag and will be given less weight than others. How much should these weigh?") .defineInRange("lightBlockWeight", 0.2, 0, Double.MAX_VALUE); diff --git a/src/main/java/com/eriksonn/createaeronautics/index/CADimensions.java b/src/main/java/com/eriksonn/createaeronautics/index/CADimensions.java index f22b2fc..29b0a53 100644 --- a/src/main/java/com/eriksonn/createaeronautics/index/CADimensions.java +++ b/src/main/java/com/eriksonn/createaeronautics/index/CADimensions.java @@ -7,8 +7,7 @@ import net.minecraftforge.event.RegistryEvent; public final class CADimensions { - public CADimensions() - {} + public CADimensions() {} public void registerDimension(RegistryEvent.NewRegistry e) { Registry.register(Registry.CHUNK_GENERATOR, AirshipDimensionManager.CHUNK_GENERATOR_ID, ChunkGenerator.CODEC); } diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/AbstractContraptionRigidbody.java b/src/main/java/com/eriksonn/createaeronautics/physics/AbstractContraptionRigidbody.java index 49b344f..fa13543 100644 --- a/src/main/java/com/eriksonn/createaeronautics/physics/AbstractContraptionRigidbody.java +++ b/src/main/java/com/eriksonn/createaeronautics/physics/AbstractContraptionRigidbody.java @@ -1,10 +1,14 @@ package com.eriksonn.createaeronautics.physics; +import com.eriksonn.createaeronautics.physics.collision.shape.ICollisionShape; +import com.simibubi.create.content.contraptions.components.structureMovement.AbstractContraptionEntity; import com.simibubi.create.content.contraptions.components.structureMovement.Contraption; import net.minecraft.util.math.BlockPos; import net.minecraft.util.math.vector.Vector3d; import net.minecraft.world.gen.feature.template.Template; +import java.util.HashMap; +import java.util.List; import java.util.Map; public abstract class AbstractContraptionRigidbody implements IRigidbody{ @@ -26,6 +30,8 @@ public void generateMassDependentParameters(Contraption contraption,Vector3d off localMass+=blockMass; } } + + localCenterOfMass=localCenterOfMass.scale(1.0/localMass); for (Map.Entry entry : contraption.getBlocks().entrySet()) { if (!entry.getValue().state.isAir()) { @@ -45,4 +51,19 @@ public void generateMassDependentParameters(Contraption contraption,Vector3d off } public double getLocalMass() { return localMass; } public Vector3d getLocalCenterOfMass(){ return localCenterOfMass; } + + //#region Collision + HashMap> collisionShapes = new HashMap<>(); + + public HashMap> getCollisionShapes() { + return collisionShapes; + } + + public void setCollisionShapes(HashMap> collisionShapes) { + this.collisionShapes = collisionShapes; + } + //#endregion + + public abstract AbstractContraptionEntity getContraption(); + abstract public Vector3d getPlotOffset(); } diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/IRigidbody.java b/src/main/java/com/eriksonn/createaeronautics/physics/IRigidbody.java index c98c565..25a53b8 100644 --- a/src/main/java/com/eriksonn/createaeronautics/physics/IRigidbody.java +++ b/src/main/java/com/eriksonn/createaeronautics/physics/IRigidbody.java @@ -28,7 +28,7 @@ public interface IRigidbody { //#region Interaction void addForce(Vector3d pos,Vector3d force); void addGlobalForce(Vector3d pos,Vector3d force); - void addVelocity(Vector3d pos,Vector3d velocity); - void addGlobalVelocity(Vector3d pos,Vector3d velocity); + void applyImpulse(Vector3d pos, Vector3d velocity); + void applyGlobalImpulse(Vector3d pos, Vector3d velocity); //#endregion } diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/SimulatedContraptionRigidbody.java b/src/main/java/com/eriksonn/createaeronautics/physics/SimulatedContraptionRigidbody.java index 3b5fa81..f3a68b2 100644 --- a/src/main/java/com/eriksonn/createaeronautics/physics/SimulatedContraptionRigidbody.java +++ b/src/main/java/com/eriksonn/createaeronautics/physics/SimulatedContraptionRigidbody.java @@ -6,8 +6,17 @@ import com.eriksonn.createaeronautics.contraptions.AirshipManager; import com.eriksonn.createaeronautics.blocks.propeller_bearing.PropellerBearingTileEntity; import com.eriksonn.createaeronautics.index.CABlocks; +import com.eriksonn.createaeronautics.index.CAConfig; import com.eriksonn.createaeronautics.index.CATileEntities; import com.eriksonn.createaeronautics.particle.PropellerAirParticleData; +import com.eriksonn.createaeronautics.physics.collision.detection.Contact; +import com.eriksonn.createaeronautics.physics.collision.detection.ICollisionDetector; +import com.eriksonn.createaeronautics.physics.collision.detection.impl.GJKCollisionDetector; +import com.eriksonn.createaeronautics.physics.collision.detection.impl.SphereAABBCollisionDetector; +import com.eriksonn.createaeronautics.physics.collision.detection.impl.SphereCollisionDetector; +import com.eriksonn.createaeronautics.physics.collision.resolution.IIterativeManifoldSolver; +import com.eriksonn.createaeronautics.physics.collision.resolution.SequentialManifoldSolver; +import com.eriksonn.createaeronautics.physics.collision.shape.MeshCollisionShapeGenerator; import com.simibubi.create.AllTileEntities; import com.simibubi.create.content.contraptions.components.fan.EncasedFanTileEntity; import com.simibubi.create.content.contraptions.components.structureMovement.AbstractContraptionEntity; @@ -29,12 +38,14 @@ import java.util.*; +import static com.eriksonn.createaeronautics.physics.PhysicsUtils.deltaTime; + public class SimulatedContraptionRigidbody extends AbstractContraptionRigidbody { AirshipContraptionEntity entity; AirshipContraption contraption; public Quaternion orientation; - Vector3d momentum=Vector3d.ZERO; + public Vector3d momentum=Vector3d.ZERO; Vector3d centerOfMass=Vector3d.ZERO; double[][] inertiaTensor=new double[3][3]; @@ -67,6 +78,9 @@ public class SimulatedContraptionRigidbody extends AbstractContraptionRigidbody Vector3d inverseInertiaTensorJ; Vector3d inverseInertiaTensorK; + // manifold solver + IIterativeManifoldSolver manifoldSolver = new SequentialManifoldSolver(); + public SimulatedContraptionRigidbody(AirshipContraptionEntity entity) { orientation=Quaternion.ONE.copy(); @@ -77,7 +91,6 @@ public SimulatedContraptionRigidbody(AirshipContraptionEntity entity) momentum=Vector3d.ZERO; principalRotation =Quaternion.ONE.copy(); - angularMomentum=new Vector3d(0,800,0); //principialRotation=new Quaternion(1,2,3,4); //principialRotation.normalize(); PhysicsUtils.generateLeviCivitaTensor(); @@ -89,31 +102,30 @@ public void tryInit() contraption=entity.airshipContraption; //updateCenterOfMass(); updateLevititeBuoyancy(); + initCollision(); isInitialized=true; +// orientation=new Quaternion(new Vector3f(0, 0, 1), 180, true); +// momentum = new Vector3d(75.0, 0.0, 0.0); +// angularMomentum = new Vector3d(75, 0, 0); } } public void tick() { + + addGlobalForce(new Vector3d(0, -4.0 * getMass(), 0), Vector3d.ZERO); contraption=entity.airshipContraption; if(contraption==null) return; - generateMassDependentParameters(contraption,Vector3d.ZERO); +// centerOfMass=Vector3d.ZERO; + generateMassDependentParameters(contraption,Vector3d.ZERO); mergeMassFromSubContraptions(); - //updateCenterOfMass(); tryInit(); - //orientation=new Quaternion(0,0,0.3827f,0.9239f); - //orientation.normalize(); - - - - //updateWings(); - //updateInertia(); +// addGlobalForce(centerOfMass, new Vector3d(0,-5.0,0)); updateTileEntityInteractions(); - centerOfMass=Vector3d.ZERO; totalAccumulatedBuoyancy =0; totalAccumulatedBuoyancy += levititeBuoyancyController.apply(orientation,entity.position()); @@ -121,7 +133,7 @@ public void tick() globalForce=globalForce.add(0,-totalAccumulatedBuoyancy,0); - momentum = momentum.add(rotateQuat(localForce.scale(PhysicsUtils.deltaTime),orientation)).add(globalForce.scale(PhysicsUtils.deltaTime)); + momentum = momentum.add(rotateQuat(localForce.scale(deltaTime),orientation)).add(globalForce.scale(deltaTime)); globalForce = Vector3d.ZERO; localForce = Vector3d.ZERO; @@ -137,15 +149,36 @@ public void tick() CurrentAxisAngle+=0.01f; + angularMomentum=angularMomentum.scale(0.995); + + List contacts = findContacts(); + + manifoldSolver.preSolve(contacts); + for (int i = 0; i < CAConfig.MAX_COLLISION_ITERATIONS.get(); i++) { + manifoldSolver.solve(this, contacts, deltaTime); + } -// orientation=new Quaternion(s*CurrentAxis.x(),s*CurrentAxis.y(),s*CurrentAxis.z(),c); + endPhysicsTick(); + + } + + public void endPhysicsTick() { + Vector3d v = angularVelocity.scale(deltaTime*0.5f); + Quaternion q = new Quaternion((float)v.x,(float)v.y,(float)v.z, 1.0f); + q.mul(orientation); + orientation=q; + orientation.normalize(); + + localTorque =Vector3d.ZERO; + globalTorque =Vector3d.ZERO; entity.quat=orientation.copy(); - entity.velocity=globalVelocity.scale(PhysicsUtils.deltaTime); - entity.setDeltaMovement(globalVelocity.scale(PhysicsUtils.deltaTime)); - entity.move(globalVelocity.x*PhysicsUtils.deltaTime,globalVelocity.y*PhysicsUtils.deltaTime,globalVelocity.z*PhysicsUtils.deltaTime); + entity.velocity=globalVelocity.scale(deltaTime); + entity.setDeltaMovement(globalVelocity.scale(deltaTime)); + entity.move(globalVelocity.x* deltaTime,globalVelocity.y* deltaTime,globalVelocity.z* deltaTime); } + public void readAdditional(CompoundNBT compound, boolean spawnPacket) { orientation = readQuaternion(compound.getCompound("Orientation")); @@ -195,6 +228,7 @@ public void addSubContraption(UUID uuid,AbstractContraptionEntity newEntity) SubcontraptionRigidbody rigidbody = new SubcontraptionRigidbody(newEntity,this); subcontraptionRigidbodyMap.put(uuid, rigidbody); rigidbody.generateMassDependentParameters(newEntity.getContraption(),Vector3d.ZERO); + generateCollisionShapes(rigidbody); } public void removeSubContraption(UUID uuid) { @@ -202,7 +236,7 @@ public void removeSubContraption(UUID uuid) } public Quaternion getPartialOrientation(float partialTick) { - Vector3d v = angularVelocity.scale(partialTick*PhysicsUtils.deltaTime*0.5f); + Vector3d v = angularVelocity.scale(partialTick* deltaTime*0.5f); Quaternion q = new Quaternion((float)v.x,(float)v.y,(float)v.z, 1.0f); q.mul(orientation); q.normalize(); @@ -311,8 +345,8 @@ void updateRotation() localTorque = localTorque.add(rotateQuatReverse(globalTorque,orientation)); //torque gives a change of angular momentum over time - angularMomentum=angularMomentum.add(localTorque.scale(PhysicsUtils.deltaTime)); - double momentumMag = angularMomentum.length(); + angularMomentum=angularMomentum.add(localTorque.scale(deltaTime)); + //rotate the angular momentum into the principal reference frame and scale by the inverse of the inertia //tensor to get angular velocity in the principal frame Vector3d principalVelocity = rotateQuat(multiplyInertiaInverse(angularMomentum), principalRotation); @@ -327,26 +361,76 @@ void updateRotation() //rotate the torque back to the contraption grid Vector3d extraTorque = rotateQuatReverse(principalTorque, principalRotation); - angularMomentum = angularMomentum.add(extraTorque.scale(PhysicsUtils.deltaTime)); + double momentumMag = angularMomentum.length(); - angularVelocity=multiplyInertiaInverse(angularMomentum); + angularMomentum = angularMomentum.add(extraTorque.scale(deltaTime)); if (angularMomentum.lengthSqr() > 0)//reset the length to maintain conservation of momentum { angularMomentum=angularMomentum.normalize(); angularMomentum=angularMomentum.scale(momentumMag); + } + + angularVelocity = multiplyInertiaInverse(angularMomentum); + } + + List findContacts() { + List contacts = new ArrayList<>(); + + // to handle collisions, we need to test every collision shape against the blocks they could collide with + // first let's call an overload on each subcontraption and this rigidbody to prevent duplicate code + findContacts(this, contacts); + for (SubcontraptionRigidbody subcontraption : subcontraptionRigidbodyMap.values()) { + // repeat for number of iterations + findContacts(subcontraption, contacts); } - //angularMomentum=angularMomentum.scale(0.995); - Vector3d v = angularVelocity.scale(PhysicsUtils.deltaTime*0.5f); - Quaternion q = new Quaternion((float)v.x,(float)v.y,(float)v.z, 1.0f); - q.mul(orientation); - orientation=q; - orientation.normalize(); - localTorque =Vector3d.ZERO; - globalTorque =Vector3d.ZERO; + return contacts; + } + + ICollisionDetector collisionDetector; + + + void findContacts(AbstractContraptionRigidbody rb, List contacts) { + collisionDetector.solve(rb, contacts); + } + + void initCollision() { + + // Set collision detector based off of quality settings + switch (CAConfig.COLLISION_QUALITY.get()) { + case GJKEPA: + collisionDetector = new GJKCollisionDetector(); + break; + case SPHERE_AABB: + collisionDetector = new SphereAABBCollisionDetector(); + break; + case SPHERE: + collisionDetector = new SphereCollisionDetector(); + break; + } + + // generate collision shapes for all the rigid bodies + generateCollisionShapes(this); + for(AbstractContraptionRigidbody rb : subcontraptionRigidbodyMap.values()) { + generateCollisionShapes(rb); + } + } + + void generateCollisionShapes(AbstractContraptionRigidbody rb) { + MeshCollisionShapeGenerator generator = new MeshCollisionShapeGenerator(rb); + + AbstractContraptionEntity contraption = rb.getContraption(); + + rb.setCollisionShapes(generator.generateShapes(contraption)); + } + + @Override + public AbstractContraptionEntity getContraption() { + return entity; } + void updateSpectralDecomposition() { // attempts to perform spectral decomposition on the local inertia tensor = A @@ -604,11 +688,13 @@ public Vector3d rotateLocalInverse(Vector3d point) { } public Vector3d toLocal(Vector3d globalPoint) { - return rotateQuatReverse(globalPoint.subtract(entity.position()).subtract(centerOfMass),orientation).add(centerOfMass); + Vector3d rotationOffset = VecHelper.getCenterOf(BlockPos.ZERO); + return rotateQuatReverse(globalPoint.subtract(entity.position()).subtract(rotationOffset),orientation); } public Vector3d toGlobal(Vector3d localPoint) { - return rotateQuat(localPoint.subtract(centerOfMass),orientation).add(centerOfMass).add(entity.position()); + Vector3d rotationalOffset = VecHelper.getCenterOf(BlockPos.ZERO); + return rotateQuat(localPoint.subtract(rotationalOffset).subtract(centerOfMass),orientation).add(rotationalOffset).add(entity.position()); } public Vector3d getVelocity() { @@ -616,9 +702,8 @@ public Vector3d getVelocity() { } public Vector3d getVelocityAtPoint(Vector3d pos) { - return globalVelocity.add(pos.cross(angularVelocity)); + return globalVelocity.add(rotate(pos.cross((angularVelocity)))); } - public Vector3d getAngularVelocity() { return angularVelocity; } @@ -635,13 +720,18 @@ public void addGlobalForce(Vector3d force, Vector3d pos) } - public void addVelocity(Vector3d pos, Vector3d velocity) { + public void applyImpulse(Vector3d pos, Vector3d impulse) { + momentum = momentum.add(impulse); + globalVelocity = momentum.scale(1.0 / getMass()); + Vector3d additionalAngularMomentum = rotateInverse(impulse).cross(pos); + angularMomentum = angularMomentum.add(additionalAngularMomentum); + updateRotation(); } - public void addGlobalVelocity(Vector3d pos, Vector3d velocity) { - + public void applyGlobalImpulse(Vector3d pos, Vector3d impulse) { + applyImpulse(toLocal(pos), impulse); } Vector3d getLocalVelocityAtPosition(Vector3d pos) @@ -679,7 +769,7 @@ Vector3d getForceEncasedFan(BlockPos pos,EncasedFanTileEntity te) entity.level.addParticle(new PropellerAirParticleData(new Vector3i(vector3d.x, vector3d.y, vector3d.z)), pPos.x, pPos.y, pPos.z, veloVector.x, veloVector.y, veloVector.z); } - return direction.scale(magnitude); + return direction.scale(-magnitude); } Vector3d getFacingVector(BlockState state) @@ -691,7 +781,10 @@ Vector3d getLocalCoordinate(BlockPos pos) { Vector3d p=new Vector3d(pos.getX(),pos.getY(),pos.getZ()); return p.subtract(centerOfMass); - + } + Vector3d getLocalCoordinate(Vector3d pos) + { + return pos.subtract(centerOfMass); } class BuoyancyController { diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/SubcontraptionRigidbody.java b/src/main/java/com/eriksonn/createaeronautics/physics/SubcontraptionRigidbody.java index 34604c9..6cc8c92 100644 --- a/src/main/java/com/eriksonn/createaeronautics/physics/SubcontraptionRigidbody.java +++ b/src/main/java/com/eriksonn/createaeronautics/physics/SubcontraptionRigidbody.java @@ -86,13 +86,13 @@ public void addGlobalForce(Vector3d pos, Vector3d force) { } - public void addVelocity(Vector3d pos, Vector3d velocity) { - parentRigidbody.addVelocity(toParent(pos),entity.applyRotation(velocity,0)); + public void applyImpulse(Vector3d pos, Vector3d velocity) { + parentRigidbody.applyImpulse(toParent(pos),entity.reverseRotation(velocity,0)); } - public void addGlobalVelocity(Vector3d pos, Vector3d velocity) { - parentRigidbody.addGlobalVelocity(toParent(pos),velocity); + public void applyGlobalImpulse(Vector3d pos, Vector3d velocity) { + parentRigidbody.applyGlobalImpulse(toParent(pos),velocity); } Vector3d toParent(Vector3d point) { @@ -104,4 +104,14 @@ Vector3d fromParent(Vector3d point) Vector3d entityOffsetPosition = entity.position().subtract(parentRigidbody.getPlotOffset()); return entity.reverseRotation(point.subtract(entityOffsetPosition),0); } + + @Override + public AbstractContraptionEntity getContraption() { + return entity; + } + + @Override + public Vector3d getPlotOffset() { + return parentRigidbody.getPlotOffset(); + } } diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/api/ICustomCollisionShape.java b/src/main/java/com/eriksonn/createaeronautics/physics/api/ICustomCollisionShape.java new file mode 100644 index 0000000..9aa0d55 --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/api/ICustomCollisionShape.java @@ -0,0 +1,26 @@ +package com.eriksonn.createaeronautics.physics.api; + +import com.eriksonn.createaeronautics.physics.collision.shape.CollisionQuality; +import com.eriksonn.createaeronautics.physics.collision.shape.ICollisionShape; + +import java.util.List; + +/** + * Interface for BlockEntities to have a custom collision shape on simulated contraptions. + */ +public interface ICustomCollisionShape { + + /** + * Determines if this collision shape should take priority over quality setting changes. + * @return True if this collision shape should take priority over the default collision shape generator. + */ + boolean takesPriority(CollisionQuality quality); + + /** + * Generates the collision shape for this block entity. + * @param quality The quality of the collision detection. + * @return The collision shape to be used for this block entity. + */ + List generateCollisionShape(CollisionQuality quality); + +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/api/IHotAirProvider.java b/src/main/java/com/eriksonn/createaeronautics/physics/api/IHotAirProvider.java new file mode 100644 index 0000000..02bf105 --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/api/IHotAirProvider.java @@ -0,0 +1,13 @@ +package com.eriksonn.createaeronautics.physics.api; + +/** + * Phyiscs-related interface for BlockEntities that can produce hot air for hot air balloons. + */ +public interface IHotAirProvider { + + /** + * Gets the current hot air output of the object. + */ + double getHotAirOutput(); + +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/api/IMassProvider.java b/src/main/java/com/eriksonn/createaeronautics/physics/api/IMassProvider.java new file mode 100644 index 0000000..1d2aaff --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/api/IMassProvider.java @@ -0,0 +1,13 @@ +package com.eriksonn.createaeronautics.physics.api; + +/** + * Phyiscs-related interface for BlockEntities with variable mass. + */ +public interface IMassProvider { + + /** + * Gets the current of the object. + */ + public double getMass(); + +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/api/IThrustProvider.java b/src/main/java/com/eriksonn/createaeronautics/physics/api/IThrustProvider.java new file mode 100644 index 0000000..8373619 --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/api/IThrustProvider.java @@ -0,0 +1,10 @@ +package com.eriksonn.createaeronautics.physics.api; + +/** + * Phyiscs-related interface for BlockEntities that provide force. + */ +public interface IThrustProvider { + + // TODO: Fix this + +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/AbstractContraptionBasedCollisionDetector.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/AbstractContraptionBasedCollisionDetector.java new file mode 100644 index 0000000..f7af31b --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/AbstractContraptionBasedCollisionDetector.java @@ -0,0 +1,84 @@ +package com.eriksonn.createaeronautics.physics.collision.detection; + +import com.eriksonn.createaeronautics.physics.AbstractContraptionRigidbody; +import com.eriksonn.createaeronautics.physics.collision.resolution.IIterativeManifoldSolver; +import com.eriksonn.createaeronautics.physics.collision.shape.ICollisionShape; +import com.eriksonn.createaeronautics.physics.collision.shape.MeshCollisionShapeGenerator; +import net.minecraft.block.Block; +import net.minecraft.block.BlockState; +import net.minecraft.block.Blocks; +import net.minecraft.util.math.AxisAlignedBB; +import net.minecraft.util.math.BlockPos; +import net.minecraft.world.World; +import net.minecraft.world.gen.feature.template.Template; + +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +abstract public class AbstractContraptionBasedCollisionDetector implements ICollisionDetector { + + @Override + public List solve(AbstractContraptionRigidbody rb, List contacts) { +// iterate over all collision shapes + final HashMap> collisionShapes = rb.getCollisionShapes(); + + World contraptionLevel = rb.getContraption().level; + + for (Map.Entry> shapeEntry : collisionShapes.entrySet()) { + + // check if this block is not fully surrounded + for (ICollisionShape shape : shapeEntry.getValue()) { + + // now that we're iterating over every shape, get it's world space bounds that it occupies + AxisAlignedBB bounds = shape.getBounds(); + + // inflate it by a tiny amount to account for floating point errors + bounds = bounds.inflate(0.1); + + // now we need to iterate over every block in the world within the bounds + for(int x = (int)Math.floor(bounds.minX); x <= Math.ceil(bounds.maxX); x++) { + for(int y = (int)Math.floor(bounds.minY); y <= Math.ceil(bounds.maxY); y++) { + for(int z = (int)Math.floor(bounds.minZ); z <= Math.ceil(bounds.maxZ); z++) { + + // get the block at the current position + BlockPos worldPos = new BlockPos(x, y, z); + + // if the block is not air, we need to check for collisions + BlockState state = contraptionLevel.getBlockState(worldPos); + Block block = state.getBlock(); + if(block == Blocks.AIR) continue; + + // get the collision shape of the block + List blockShape = new MeshCollisionShapeGenerator(rb).generateFromBlock(contraptionLevel, worldPos, state, false); + + // if the block has no collision shape, we can skip it + if(blockShape.isEmpty()) continue; + + // now we need to iterate over every shape in the block + for(ICollisionShape blockShapeShape : blockShape) { + // use GJKEPA to test for collisions + Manifold manifold = test(rb, shape, blockShapeShape, shapeEntry.getKey(), worldPos); + + // if there's a collision, solve it + if(manifold != null) { + contacts.add(new Contact(manifold, state, state, rb)); + + // hi you found my super secret comment, and my debug code + int a = 2 + 2; + } + } + } + } + } + } + } + + return contacts; + } + + public abstract Manifold test(AbstractContraptionRigidbody rb, ICollisionShape shape, ICollisionShape otherShape, BlockPos localBlockPos, BlockPos worldPos); + + + +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Contact.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Contact.java new file mode 100644 index 0000000..efef52b --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Contact.java @@ -0,0 +1,46 @@ +package com.eriksonn.createaeronautics.physics.collision.detection; + +import com.eriksonn.createaeronautics.physics.AbstractContraptionRigidbody; +import net.minecraft.block.BlockState; +import net.minecraft.util.math.vector.Vector3d; + +/** + * Represents a contact between two contraptions, containing both a manifold and information about the colliding blocks. + */ +public class Contact { + public Manifold manifold; + public BlockState blockA, blockB; + public AbstractContraptionRigidbody rigidbody; + + /** + * Impulse accumulated in normal direction + */ + public double normalImpulse = 0; + + /** + * Impulse accumulated in the tangent direction + */ + public double tangentImpulse = 0; + + /** + * Effective mass in the normal direction + */ + public double normalMass = 0; + + /** + * Inverse mass + */ + public double inverseMass = 0; + + /** + * Point of collision + */ + public Vector3d localContactPoint = Vector3d.ZERO; + + public Contact(Manifold manifold, BlockState blockA, BlockState blockB, AbstractContraptionRigidbody rigidbody) { + this.manifold = manifold; + this.blockA = blockA; + this.blockB = blockB; + this.rigidbody = rigidbody; + } +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/EvolutionResult.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/EvolutionResult.java new file mode 100644 index 0000000..28e995d --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/EvolutionResult.java @@ -0,0 +1,16 @@ +package com.eriksonn.createaeronautics.physics.collision.detection; + +import net.minecraft.util.math.vector.Vector3d; + +/** + * Represents a result for simplex evolution. + */ +public class EvolutionResult { + public boolean complete; + public Vector3d direction; + + public EvolutionResult(boolean complete, Vector3d direction) { + this.complete = complete; + this.direction = direction; + } +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/GJKEPA.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/GJKEPA.java new file mode 100644 index 0000000..2959e2a --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/GJKEPA.java @@ -0,0 +1,600 @@ +package com.eriksonn.createaeronautics.physics.collision.detection; + +import com.eriksonn.createaeronautics.physics.collision.shape.ICollisionShape; +import net.minecraft.util.math.vector.Quaternion; +import net.minecraft.util.math.vector.Vector3d; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.Objects; + +/** + * GJK & Expanding Polytope algorithm. + * + * @author RyanHCode + */ +public class GJKEPA { + + /** + * Default max simplex evolution iterations before canceling and returning. + */ + public static int MAX_SIMPLEX_ITERATIONS = 20; + + public static final double EPSILON = 0.00001f; + + /** + * Creates an ArrayList of Vector3d's + * + * @param points The points to add to the list + * @return The list of points + */ + public static ArrayList Vector3dListOf(Vector3d... points) { + return new ArrayList(Arrays.asList(points)); + } + + /** + * Checks if two vectors are in the same direction. + * + * @param a The first vector. + * @param b The second vector. + * @return True if the vectors are in the same direction. + */ + public static boolean sameDirection(Vector3d a, Vector3d b) { + return a.dot(b) > 0; + } + + /** + * Evolves the given simplex. + * @param simplex The existing simplex. + * @param direction The direction vector. + */ + private static EvolutionResult evolve(Simplex simplex, Vector3d direction) { + // Based on the length of the already given simplex, determine how to evolve it + switch (simplex.size()) { + + // The simplex has a line. + case 2: { + return evolveLine(simplex, direction); + } + + case 3: { + return evolveTriangle(simplex, direction); + } + + case 4: { + return evolveTetrahedron(simplex, direction); + } + + } + + throw new IllegalStateException("Invalid simplex size: " + simplex.size()); + } + + /** + * Evolves the given line simplex. + * @param simplex The existing simplex. + * @param direction The direction vector. + */ + private static EvolutionResult evolveLine(Simplex simplex, Vector3d direction) { + // Get points on simplex + Support a = simplex.get(0); + Support b = simplex.get(1); + + // Vector from a -> b, or the line vector + Vector3d ab = b.difference.subtract(a.difference); + + // Vector from a -> origin + Vector3d ao = Vector3d.ZERO.subtract(a.difference); + + if(sameDirection(ab, ao)) { + direction = ab.cross(ao).cross(ab); + } else { + simplex.clear(); + simplex.add(0, a); + if(simplex.size() > 4) simplex.remove(simplex.size() - 1); + direction = ao; + } + + return new EvolutionResult(false, direction); + } + + /** + * Evolves the given triangle simplex. + * @param simplex The existing simplex. + * @param direction The direction vector. + */ + private static EvolutionResult evolveTriangle(Simplex simplex, Vector3d direction) { + // Get points on simplex + Support a = simplex.get(0); + Support b = simplex.get(1); + Support c = simplex.get(2); + + // Vector from a -> b, an edge vector + Vector3d ab = b.difference.subtract(a.difference); + + // Vector from a -> b, or the line vector + Vector3d ac = c.difference.subtract(a.difference); + + // Vector from a -> origin + Vector3d ao = Vector3d.ZERO.subtract(a.difference); + + // Cross of all vectors for abc + Vector3d abc = ab.cross(ac); + + // If abc cross ac is in the same direction as a -> origin + if (sameDirection(abc.cross(ac), ao)) { + if (sameDirection(ac, ao)) { + simplex.clear(); + simplex.addAll(new Simplex(a, c)); + direction = ac.cross(ao).cross(ac); + } else { + return evolveLine(new Simplex(a, b), direction); + } + } else { + if (sameDirection(ab.cross(abc), ao)) { + return evolveLine(new Simplex(a, b), direction); + } else { + if (sameDirection(abc, ao)) { + direction = abc; + } + + else { + simplex.clear(); + simplex.addAll(new Simplex(a, c, b)); + direction = Vector3d.ZERO.subtract(abc); + } + } + } + + return new EvolutionResult(false, direction); + } + + /** + * Evolves the given tetrahedron simplex. + * + * @param simplex The existing simplex. + * @param direction The direction vector. + */ + private static EvolutionResult evolveTetrahedron(Simplex simplex, Vector3d direction) { + + // First, get all points from the simplex that we are assured to have + Support a = simplex.get(0), + b = simplex.get(1), + c = simplex.get(2), + d = simplex.get(3); + + // Get their coordinates + Vector3d axyz = a.difference, + bxyz = b.difference, + cxyz = c.difference, + dxyz = d.difference; + + // Next, get the direction from A to the other points & origin + Vector3d ab = bxyz.subtract(axyz), + ac = cxyz.subtract(axyz), + ad = dxyz.subtract(axyz), + ao = Vector3d.ZERO.subtract(axyz); + + // Then get the crosses + Vector3d abc = ab.cross(ac), + adb = ad.cross(ab), + acd = ac.cross(ad); + + // If abc is in the same direction as a -> origin, return the evolution of the triangle a, b, c + if (sameDirection(abc, ao)) { + return evolveTriangle(new Simplex(a, b, c), direction); + } + + // If acd is in the same direction as a -> origin, return the evolution of the triangle a, c, d + if (sameDirection(acd, ao)) { + return evolveTriangle(new Simplex(a, c, d), direction); + } + + // If adb is in the same direction as a -> origin, return the evolution of the triangle a, d, b + if (sameDirection(adb, ao)) { + return evolveTriangle(new Simplex(a, d, b), direction); + } + + return new EvolutionResult(true, direction); + } + + /** + * Test collision between two shapes with GJK & EPA. + * + * @param a The first shape. + * @param b The second shape. + * @return A nullable manifold collision result. + */ + public static Manifold collisionTest(ICollisionShape a, ICollisionShape b, int maxIterations) { + Simplex simplex = new Simplex(); + + // Starting direction for support points. + Vector3d direction = a.getCenter().subtract(b.getCenter()).normalize(); + + // Find initial support point, the first point of the simplex. + Support initialSupport = Support.generate(a, b, direction); + + // Add the initial support point to the simplex. + simplex.add(0, initialSupport); + + // Next direction is the direction to the origin from the initial support point. + direction = Vector3d.ZERO.subtract(initialSupport.difference).normalize(); + + // Iterate until the maximum iterations are reached or success/failure is determined. + for(int i = 0; i < maxIterations; i++) { + Support support = Support.generate(a, b, direction); + + if(support.difference.dot(direction) < 0) { + // This means that the point did NOT pass the origin + // Therefore is it impossible for the minkowski difference to contain the origin. + // The shapes do not collide. + return null; + } + + // Add this vertex to the simplex. + simplex.add(0, support); + if(simplex.size() > 4) simplex.remove(simplex.size() - 1); + + // Evolve the simplex. + EvolutionResult result = evolve(simplex, direction); + direction = result.direction.normalize(); + + if(result.complete) { + try { + return EPA(simplex, a, b, EPSILON); + } catch(Exception e) { + // print output + System.out.println("EPA failed"); + System.out.println("Simplex: " + simplex); + System.out.println("Direction: " + direction); + System.out.println("Support: " + support); + System.out.println("Result: " + result); + e.printStackTrace(); + return null; + } + } + } + + return null; + } + + /** + * Normal calculation result for EPA + */ + private static class EPANormalResult { + public final List normals; + public int minimumDistanceTriangle; + public final double minimumDistance; + + public EPANormalResult(List normals, int minimumDistanceTriangle, double minimumDistance) { + this.normals = normals; + this.minimumDistanceTriangle = minimumDistanceTriangle; + this.minimumDistance = minimumDistance; + } + + /** + * A normal result entry, consiting of a normal and the distance from the origin of this face. + */ + public static class NormalEntry { + public final Vector3d normal; + public final double distance; + public final int a, b, c; + + public NormalEntry(int a, int b, int c, Vector3d normal, double distance) { + this.a = a; + this.b = b; + this.c = c; + this.normal = normal; + this.distance = distance; + } + } + } + + /** + * An edge with a start and end index + */ + private static class Edge { + public final int start, end; + + public Edge(int start, int end) { + this.start = start; + this.end = end; + } + + @Override + public boolean equals(Object other) { + if (this == other) return true; + if (other == null || getClass() != other.getClass()) return false; + Edge edge = (Edge) other; + return (start == edge.start && end == edge.end) || (start == edge.end && end == edge.start); + } + + @Override + public int hashCode() { + return Objects.hash(start, end); + } + } + + /** + * Expanding polytope algorithm for obtaining collision normal vector. + * + * @param simplex The given tetrahedron simplex from GJK evaluation. + * @param a The first shape. + * @param b The second shape. + */ + public static Manifold EPA(Simplex simplex, ICollisionShape a, ICollisionShape b, double epsilon) { + + // At this point in the algorithm, the simplex is guaranteed to: + // * Have a size of 4, as a tetrahedron. + // * Be completely made of support points in the minkowski difference. + // * Contain the origin + + // As the simplex is now going to be expanded, it will be classified as a polytope. + Simplex polytope = simplex; + + // Triangles represented as pairs of 3 indexes of vertices in the polytope. + List triangles = new ArrayList(Arrays.asList( + 0, 1, 2, + 0, 3, 1, + 0, 2, 3, + 1, 3, 2 + )); + + // Get the normals of the triangles. + EPANormalResult normalResult = getNormals(polytope, triangles); + + // Get the normal entries + List normals = normalResult.normals; + + // Minimum distance from the last iteration. + double minDistance = Double.MAX_VALUE; + + // Number of iterations + int iterations = 0; + + while (minDistance == Double.MAX_VALUE) { + // Get the closest face + int closestFace = normalResult.minimumDistanceTriangle * 3; + + minDistance = normals.get(normalResult.minimumDistanceTriangle).distance; + + // Find a new support point in the direction of the normal + Support support = Support.generate(a, b, normals.get(normalResult.minimumDistanceTriangle).normal); + + double minFaceDistance = normals.get(normalResult.minimumDistanceTriangle).normal.dot(support.difference); + + if (Math.abs(minFaceDistance - minDistance) > epsilon) { + minDistance = Double.MAX_VALUE; + + // Add the support point to the polytope + polytope.add(support); + + // Index of the new vertex + int supportIndex = polytope.size() - 1; + + // List of unique edges(for forming new triangles while expanding. + List uniqueEdges = new ArrayList(); + + // List of duplicate edges + List duplicateEdges = new ArrayList(); + + // List of triangles to remove + List trianglesToRemove = new ArrayList(); + + uniqueEdges.add(new Edge(triangles.get(closestFace), triangles.get(closestFace + 1))); + uniqueEdges.add(new Edge(triangles.get(closestFace), triangles.get(closestFace + 2))); + uniqueEdges.add(new Edge(triangles.get(closestFace + 1), triangles.get(closestFace + 2))); + + // Remove the closest face + // Removing same index due to shifting + triangles.remove(closestFace); + triangles.remove(closestFace); + triangles.remove(closestFace); + normals.remove(normalResult.minimumDistanceTriangle); + + // Initial size of the triangles list + int trianglesSize = triangles.size(); + + normalResult = getNormals(polytope, triangles); + normals = normalResult.normals; + + // Remove all faces that can see this support point + for (int i = 0; i < trianglesSize; i += 3) { + // Get the normal entry at this triangle + EPANormalResult.NormalEntry entry = normals.get(i / 3); + + // Check if the support point is in the positive halfspace of this normal + boolean seenBySupport = entry.normal.dot(support.difference.subtract(polytope.get(entry.a).difference)) > 0; + + // Remove this triangle + if (seenBySupport) { + // Add an edge to the unique edges if it doesn't exist + Edge ab = new Edge(entry.a, entry.b); + Edge bc = new Edge(entry.b, entry.c); + Edge ca = new Edge(entry.c, entry.a); + + if (uniqueEdges.contains(ab)) { + duplicateEdges.add(ab); + uniqueEdges.remove(ab); + } else if (!duplicateEdges.contains(ab)) { + uniqueEdges.add(ab); + } + + if (uniqueEdges.contains(bc)) { + duplicateEdges.add(bc); + uniqueEdges.remove(bc); + } else if (!duplicateEdges.contains(bc)) { + uniqueEdges.add(bc); + } + + if (uniqueEdges.contains(ca)) { + duplicateEdges.add(ca); + uniqueEdges.remove(ca); + } else if (!duplicateEdges.contains(ca)) { + uniqueEdges.add(ca); + } + + triangles.remove(i); + triangles.remove(i); + triangles.remove(i); + + normals.remove(i / 3); + i -= 3; + trianglesSize -= 3; + } + } + + // Iterate over the unique edges to form new triangles + // (this is the expanding part) + for (Edge edge : uniqueEdges) { + // Get the vertices of the edge + int pointA = edge.start; + int pointB = edge.end; + int pointC = supportIndex; + + // Add the triangle! + triangles.add(pointA); + triangles.add(pointB); + triangles.add(pointC); + } + + // Get the normals of the triangles again. + normalResult = getNormals(polytope, triangles); + + // Get the normal entries + normals = normalResult.normals; + + if (iterations++ > GJKEPA.MAX_SIMPLEX_ITERATIONS) { + break; + } + } + } + + if(minDistance == Double.POSITIVE_INFINITY) { + return null; + } + + Vector3d collisionNormal = normals.get(normalResult.minimumDistanceTriangle).normal.normalize(); + double collisionDepth = minDistance + epsilon; + + // The collision depth is now known + // We can now calculate the contact points for colliders A and B + // First, find the support points that formed the last triangle + Support supportA = polytope.get(normals.get(normalResult.minimumDistanceTriangle).a), + supportB = polytope.get(normals.get(normalResult.minimumDistanceTriangle).b), + supportC = polytope.get(normals.get(normalResult.minimumDistanceTriangle).c); + + // Project the origin onto the plane of the triangle in minkowski space + Vector3d originProjection = collisionNormal.scale(-collisionDepth); + + // Get the barycentric coordinates of the origin projection on both triangles the difference was formed from + Vector3d barycentric = getBarycentricCoordinates(originProjection, supportA.difference, supportB.difference, supportC.difference, collisionNormal); + + // Use the barycentric coordinates to find the contact points on both colliders + Vector3d contactPointA = supportA.pointA.scale(barycentric.x).add(supportB.pointA.scale(barycentric.y)).add(supportC.pointA.scale(barycentric.z)); + Vector3d contactPointB = supportA.pointB.scale(barycentric.x).add(supportB.pointB.scale(barycentric.y)).add(supportC.pointB.scale(barycentric.z)); + + // If the barycentric coordinates are not valid, then the origin projection is not on the triangle + if(barycentric.x < -epsilon || barycentric.y < -epsilon || barycentric.z < -epsilon || barycentric.x + barycentric.y + barycentric.z > 1 + (epsilon * 3)) { + return null; + } + + if(collisionNormal.equals(Vector3d.ZERO) || Double.isNaN(contactPointA.x)) { + return null; + } + + return new Manifold(collisionNormal, collisionDepth, contactPointA, contactPointB); + + } + + /** + * Calculates the barycentric coordinates of a point on a given triangle. + * + * @param point The point to calculate the barycentric coordinates of. Assumed to be on the triangle + * @param a The first vertex of the triangle + * @param b The second vertex of the triangle + * @param c The third vertex of the triangle + * @param normal The normal of the triangle + * @return The barycentric coordinates of the point on the triangle + */ + public static Vector3d getBarycentricCoordinates(Vector3d point, Vector3d a, Vector3d b, Vector3d c, Vector3d normal) { + double areaABC = normal.dot((b.subtract(a).cross((c.subtract(a))))); + double areaPBC = normal.dot((b.subtract(point).cross((c.subtract(point))))); + double areaPCA = normal.dot((c.subtract(point).cross((a.subtract(point))))); + + Vector3d barycentric = new Vector3d(areaPBC / areaABC, areaPCA / areaABC, 1.0 - (areaPBC / areaABC) - (areaPCA / areaABC)); + + return barycentric; + } + + /** + * Calculate the normals of the given triangles on a polytope. + * @param polytope The given polytope. + * @param triangles The triangles to calculate the normals of. + * @return The normals of the given triangles. + */ + private static EPANormalResult getNormals(Simplex polytope, List triangles) { + List normals = new ArrayList<>(); + + double minimumDistance = Double.MAX_VALUE; + int minimumDistanceTriangle = 0; + + for(int i = 0; i < triangles.size(); i += 3) { + // Get all 3 vertices of the triangle. + int a = triangles.get(i), + b = triangles.get(i + 1), + c = triangles.get(i + 2); + + // Get the normal of this triangle. + Vector3d normal = polytope.get(b).difference + .subtract(polytope.get(a).difference) + .cross( + polytope.get(c).difference + .subtract(polytope.get(a).difference) + ) + .normalize(); + + // Get the distance from the origin to the plane of this triangle. + double distance = polytope.get(a).difference.dot(normal); + + // If this distance is less than 0, reverse the normal to face outwards relative to the origin. + // Distance will need to be negated aswell + if(distance < 0) { + normal = normal.scale(-1); + distance = -distance; + } + + // If this distance is less than the minimum distance, set this as the minimum distance. + if(distance < minimumDistance) { + minimumDistance = distance; + minimumDistanceTriangle = i / 3; + } + + // Add this normal to the list of normals. + normals.add(new EPANormalResult.NormalEntry(a, b, c, normal, distance)); + } + + return new EPANormalResult(normals, minimumDistanceTriangle, minimumDistance); + } + + public static Vector3d rotateQuaternion (Vector3d vector, Quaternion rotation) + { + Quaternion vectorAsQuaternion = new Quaternion ((float)vector.x, (float)vector.y, (float)vector.z, 0.0f); + Quaternion mut = rotation.copy(); + vectorAsQuaternion.mul(mut) ; + mut.conj(); + mut.mul(vectorAsQuaternion); + return new Vector3d(mut.i(), mut.j(), mut.k()); + } + + public static Vector3d rotateQuaternionReverse(Vector3d vector, Quaternion Q) { + Quaternion vectorAsQuaternion = new Quaternion((float)vector.x, (float)vector.y, (float)vector.z, 0.0f) ; + Quaternion mut = Q.copy(); + mut.conj(); + vectorAsQuaternion.mul(mut); + mut.conj(); + mut.mul(vectorAsQuaternion); + return new Vector3d(mut.i(), mut.j(), mut.k()); + } +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/ICollisionDetector.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/ICollisionDetector.java new file mode 100644 index 0000000..2449a20 --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/ICollisionDetector.java @@ -0,0 +1,22 @@ +package com.eriksonn.createaeronautics.physics.collision.detection; + +import com.eriksonn.createaeronautics.physics.AbstractContraptionRigidbody; +import com.eriksonn.createaeronautics.physics.SimulatedContraptionRigidbody; +import com.eriksonn.createaeronautics.physics.collision.resolution.IIterativeManifoldSolver; + +import java.util.List; + +/** + * Interface for collision detection methods, intended for use with contraption rigidbodies. + */ +public interface ICollisionDetector { + + /** + * Detects collisions between a contraption rigidbody and the world. + * @param rb the contraption rigidbody to check for collisions with the world. + * @param contacts the existing list of contacts to mutate. + * @return the mutated list of contacts. + */ + List solve(AbstractContraptionRigidbody rb, List contacts); + +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Manifold.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Manifold.java new file mode 100644 index 0000000..c06dcec --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Manifold.java @@ -0,0 +1,20 @@ +package com.eriksonn.createaeronautics.physics.collision.detection; + +import net.minecraft.util.math.vector.Vector3d; + +/** + * Represents a collision, with a position, normal vector, and contact points on both shapes. + */ +public class Manifold { + public Vector3d normal; + public double depth; + public Vector3d contactPointA; + public Vector3d contactPointB; + + public Manifold(Vector3d normal, double depth, Vector3d contactPointA, Vector3d contactPointB) { + this.normal = normal; + this.depth = depth; + this.contactPointA = contactPointA; + this.contactPointB = contactPointB; + } +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Simplex.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Simplex.java new file mode 100644 index 0000000..b54f804 --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Simplex.java @@ -0,0 +1,20 @@ +package com.eriksonn.createaeronautics.physics.collision.detection; + +import java.util.ArrayList; +import java.util.Arrays; + +/** + * Represents a Simplex or Polytope for use in GJK & EPA, in the Minkowski Difference space. + * All points contain a reference to the original points they were formed from. + */ +public class Simplex extends ArrayList { + + /** + * Constructs a simplex given a vararg of points. + * @param points A vararg of points + */ + public Simplex(Support... points) { + super(Arrays.asList(points)); + } + +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Support.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Support.java new file mode 100644 index 0000000..c7fa6f9 --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Support.java @@ -0,0 +1,66 @@ +package com.eriksonn.createaeronautics.physics.collision.detection; + +import com.eriksonn.createaeronautics.physics.collision.shape.ICollisionShape; +import net.minecraft.util.math.vector.Vector3d; + +/** + * A support point for use with the GJK & EPA algorithms. + * + * Contains both info on the point in Minkowski difference space and the individual points on both shapes that formed + * them, and the support directions used to find them. + */ +public class Support { + public Vector3d difference; + public Vector3d pointA; + public Vector3d pointB; + public Vector3d direction; + + /** + * Creates a new support point. + * @param difference The point in Minkowski difference space. + * @param pointA The point on shape A that formed the support point. + * @param pointB The point on shape B that formed the support point. + * @param direction The direction used to find the point. + */ + public Support(Vector3d difference, Vector3d pointA, Vector3d pointB, Vector3d direction) { + this.difference = difference; + this.pointA = pointA; + this.pointB = pointB; + this.direction = direction; + } + + /** + * Calculate the Minkowski difference on two shapes given a direction vector. + * + * @param a The first shape. + * @param b The second shape. + * @param direction The direction vector. + * @return The Minkowski difference. + */ + public static Vector3d minkowskiDifference(ICollisionShape a, ICollisionShape b, Vector3d direction) { + Vector3d oppositeDirection = direction.scale(-1); + Vector3d support = a.support(direction); + return support.subtract(b.support(oppositeDirection)); + } + + /** + * Generates a support point from two shapes and a direction based on the Minkowski difference of the two support points. + * + * @param a The first shape. + * @param b The second shape. + * @param direction The direction to find the support point in. + */ + public static Support generate(ICollisionShape a, ICollisionShape b, Vector3d direction) { + // Get opposite direction for difference + Vector3d oppositeDirection = direction.scale(-1); + + // Calculate support points + Vector3d supportA = a.support(direction); + Vector3d supportB = b.support(oppositeDirection); + + // Calculate difference + Vector3d minkowskiDifference = supportA.subtract(supportB); + + return new Support(minkowskiDifference, supportA, supportB, direction); + } +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Triangle.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Triangle.java new file mode 100644 index 0000000..c1ef2ee --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/Triangle.java @@ -0,0 +1,25 @@ +package com.eriksonn.createaeronautics.physics.collision.detection; + +import net.minecraft.util.math.vector.Vector3d; + +public class Triangle { + public Vector3d[] vertices; + public Vector3d normal; + + public Triangle(Vector3d[] vertices) { + this.vertices = vertices; + this.normal = getNormal(); + } + + public Triangle(Vector3d v1, Vector3d v2, Vector3d v3) { + this.vertices = new Vector3d[] {v1, v2, v3}; + this.normal = getNormal(); + } + + public Vector3d getNormal() { + Vector3d u = vertices[1].subtract(vertices[0]); + Vector3d v = vertices[2].subtract(vertices[0]); + + return u.cross(v).normalize(); + } +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/impl/GJKCollisionDetector.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/impl/GJKCollisionDetector.java new file mode 100644 index 0000000..d55cf43 --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/impl/GJKCollisionDetector.java @@ -0,0 +1,21 @@ +package com.eriksonn.createaeronautics.physics.collision.detection.impl; + +import com.eriksonn.createaeronautics.index.CAConfig; +import com.eriksonn.createaeronautics.physics.AbstractContraptionRigidbody; +import com.eriksonn.createaeronautics.physics.collision.detection.AbstractContraptionBasedCollisionDetector; +import com.eriksonn.createaeronautics.physics.collision.detection.GJKEPA; +import com.eriksonn.createaeronautics.physics.collision.detection.Manifold; +import com.eriksonn.createaeronautics.physics.collision.shape.ICollisionShape; +import net.minecraft.util.math.BlockPos; + +/** + * Collision detection using the Gilbert-Johnson-Keerthi and expanding polytope algorithms. + */ +public class GJKCollisionDetector extends AbstractContraptionBasedCollisionDetector { + + @Override + public Manifold test(AbstractContraptionRigidbody rb, ICollisionShape shape, ICollisionShape otherShape, BlockPos localBlockPos, BlockPos worldPos) { + return GJKEPA.collisionTest(shape, otherShape, CAConfig.MAX_COLLISION_ITERATIONS.get()); + } + +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/impl/SphereAABBCollisionDetector.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/impl/SphereAABBCollisionDetector.java new file mode 100644 index 0000000..98c7dcc --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/impl/SphereAABBCollisionDetector.java @@ -0,0 +1,136 @@ +package com.eriksonn.createaeronautics.physics.collision.detection.impl; + +import com.eriksonn.createaeronautics.physics.AbstractContraptionRigidbody; +import com.eriksonn.createaeronautics.physics.collision.detection.AbstractContraptionBasedCollisionDetector; +import com.eriksonn.createaeronautics.physics.collision.detection.Manifold; +import com.eriksonn.createaeronautics.physics.collision.shape.ICollisionShape; +import net.minecraft.client.gui.screen.EditSignScreen; +import net.minecraft.util.math.AxisAlignedBB; +import net.minecraft.util.math.BlockPos; +import net.minecraft.util.math.vector.Vector3d; + +/** + * Collision detection using sphere-sphere collision detection. + */ +public class SphereAABBCollisionDetector extends AbstractContraptionBasedCollisionDetector { + + public static double getVectorValueAtIndex(Vector3d vector, int index) { + switch (index) { + case 0: + return vector.x; + case 1: + return vector.y; + case 2: + return vector.z; + default: + throw new IllegalArgumentException("Index must be between 0 and 2"); + } + } + + public static Vector3d setVectorValueAtIndex(Vector3d vector, int index, double value) { + switch (index) { + case 0: + vector = new Vector3d(value, vector.y, vector.z); + break; + case 1: + vector = new Vector3d(vector.x, value, vector.z); + break; + case 2: + vector = new Vector3d(vector.x, vector.y, value); + break; + default: + throw new IllegalArgumentException("Index must be between 0 and 2"); + } + + return vector; + } + + public static Vector3d closestPointAABB(Vector3d point, AxisAlignedBB aabb) { + Vector3d q = new Vector3d(0.0, 0.0, 0.0); + + Vector3d min = new Vector3d(aabb.minX, aabb.minY, aabb.minZ); + Vector3d max = new Vector3d(aabb.maxX, aabb.maxY, aabb.maxZ); + + for( int i = 0; i < 3; i++ ) { + double v = getVectorValueAtIndex(point, i); + if( v < getVectorValueAtIndex(min, i) ) v = getVectorValueAtIndex(min, i); + if( v > getVectorValueAtIndex(max, i) ) v = getVectorValueAtIndex(max, i); + q = setVectorValueAtIndex(q, i, v); + } + return q; + } + + @Override + public Manifold test(AbstractContraptionRigidbody rb, ICollisionShape shape, ICollisionShape otherShape, BlockPos localBlockPos, BlockPos worldPos) { + Vector3d spherePositionA = rb.toGlobal(new Vector3d(localBlockPos.getX(), localBlockPos.getY(), localBlockPos.getZ()).add(0.5,0.5,0.5)/*.add(rb.getCenterOfMass())*/); + AxisAlignedBB aabb = new AxisAlignedBB(worldPos); + + if(spherePositionA.distanceToSqr(aabb.getCenter()) < 0.001) return null; + + double radiusA = 0.5; + + // Test for collision + Vector3d closestPointA = closestPointAABB(spherePositionA, aabb); + double distance = spherePositionA.distanceToSqr(closestPointA); + double radiusSum = radiusA; + if (distance < radiusSum * radiusSum) { + // Collision detected + Vector3d normal = spherePositionA.subtract(closestPointA).normalize(); + double penetration = radiusSum - Math.sqrt(distance); + + if (penetration < 0) { + return null; + } + + // Position of the collision point + Vector3d collisionPointA = spherePositionA.add(normal.scale(-radiusA)); + Vector3d collisionPointB = closestPointA; + + return new Manifold(normal.scale(-1.0), penetration, collisionPointA, collisionPointB); + } else { + return null; + } + } + + + /* + + Vector3d spherePositionA = rb.toGlobal(new Vector3d(localBlockPos.getX(), localBlockPos.getY(), localBlockPos.getZ()).add(0.5,0.5,0.5)); + Vector3d spherePositionB = new Vector3d(worldPos.getX(), worldPos.getY(), worldPos.getZ()).add(0.5,0.5,0.5); + + if(spherePositionA.distanceToSqr(spherePositionB) < 0.001) return null; + + double radiusA = 0.5; + double radiusB = 0.5; + + // Test for collision + double distance = spherePositionA.distanceToSqr(spherePositionB); + double radiusSum = radiusA + radiusB; + if (distance < radiusSum * radiusSum) { + // Collision detected + Vector3d normal = spherePositionA.subtract(spherePositionB).normalize(); + double penetration = radiusSum - Math.sqrt(distance); + + if (penetration < 0) { + return null; + } + + // Position of the collision point + Vector3d collisionPointA = spherePositionA.add(normal.scale(-radiusA)); + Vector3d collisionPointB = spherePositionB.add(normal.scale(radiusB)); + + return new Manifold(normal.scale(-1.0), penetration, collisionPointA, collisionPointB); + } else { + return null; + } + } + + + + + + */ + +} + + diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/impl/SphereCollisionDetector.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/impl/SphereCollisionDetector.java new file mode 100644 index 0000000..acb7a94 --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/detection/impl/SphereCollisionDetector.java @@ -0,0 +1,90 @@ +package com.eriksonn.createaeronautics.physics.collision.detection.impl; + +import com.eriksonn.createaeronautics.index.CAConfig; +import com.eriksonn.createaeronautics.physics.AbstractContraptionRigidbody; +import com.eriksonn.createaeronautics.physics.collision.detection.AbstractContraptionBasedCollisionDetector; +import com.eriksonn.createaeronautics.physics.collision.detection.Manifold; +import com.eriksonn.createaeronautics.physics.collision.shape.ICollisionShape; +import net.minecraft.util.math.BlockPos; +import net.minecraft.util.math.vector.Vector3d; + +/** + * Collision detection using sphere-sphere collision detection. + */ +public class SphereCollisionDetector extends AbstractContraptionBasedCollisionDetector { + + + @Override + public Manifold test(AbstractContraptionRigidbody rb, ICollisionShape shape, ICollisionShape otherShape, BlockPos localBlockPos, BlockPos worldPos) { + Vector3d spherePositionA = rb.toGlobal(new Vector3d(localBlockPos.getX(), localBlockPos.getY(), localBlockPos.getZ()).add(0.5,0.5,0.5)/*.add(rb.getCenterOfMass())*/); + Vector3d spherePositionB = new Vector3d(worldPos.getX(), worldPos.getY(), worldPos.getZ()).add(0.5,0.5,0.5); + + if(spherePositionA.distanceToSqr(spherePositionB) < 0.001) return null; + + double radiusA = 0.5; + double radiusB = 0.5; + + // Test for collision + double distance = spherePositionA.distanceToSqr(spherePositionB); + double radiusSum = radiusA + radiusB; + if (distance < radiusSum * radiusSum) { + // Collision detected + Vector3d normal = spherePositionA.subtract(spherePositionB).normalize(); + double penetration = radiusSum - Math.sqrt(distance); + + if (penetration < 0) { + return null; + } + + // Position of the collision point + Vector3d collisionPointA = spherePositionA.add(normal.scale(-radiusA)); + Vector3d collisionPointB = spherePositionB.add(normal.scale(radiusB)); + + return new Manifold(normal.scale(-1.0), penetration, collisionPointA, collisionPointB); + } else { + return null; + } + } + + + /* + + Vector3d spherePositionA = rb.toGlobal(new Vector3d(localBlockPos.getX(), localBlockPos.getY(), localBlockPos.getZ()).add(0.5,0.5,0.5)); + Vector3d spherePositionB = new Vector3d(worldPos.getX(), worldPos.getY(), worldPos.getZ()).add(0.5,0.5,0.5); + + if(spherePositionA.distanceToSqr(spherePositionB) < 0.001) return null; + + double radiusA = 0.5; + double radiusB = 0.5; + + // Test for collision + double distance = spherePositionA.distanceToSqr(spherePositionB); + double radiusSum = radiusA + radiusB; + if (distance < radiusSum * radiusSum) { + // Collision detected + Vector3d normal = spherePositionA.subtract(spherePositionB).normalize(); + double penetration = radiusSum - Math.sqrt(distance); + + if (penetration < 0) { + return null; + } + + // Position of the collision point + Vector3d collisionPointA = spherePositionA.add(normal.scale(-radiusA)); + Vector3d collisionPointB = spherePositionB.add(normal.scale(radiusB)); + + return new Manifold(normal.scale(-1.0), penetration, collisionPointA, collisionPointB); + } else { + return null; + } + } + + + + + + */ + +} + + diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/resolution/IIterativeManifoldSolver.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/resolution/IIterativeManifoldSolver.java new file mode 100644 index 0000000..e815858 --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/resolution/IIterativeManifoldSolver.java @@ -0,0 +1,34 @@ +package com.eriksonn.createaeronautics.physics.collision.resolution; + +import com.eriksonn.createaeronautics.physics.IRigidbody; +import com.eriksonn.createaeronautics.physics.collision.detection.Contact; +import com.eriksonn.createaeronautics.physics.collision.detection.Manifold; +import net.minecraft.block.BlockState; + +import java.util.List; + +/** + * Interface for iterative solvers of collision manifolds. + */ +public interface IIterativeManifoldSolver { + + /** + * Initializes the solver for a new frame. + * @param contacts the contacts + */ + void preSolve(List contacts); + + /** + * Solves the collision contact. + * + * @param rigidbody the rigidbody + * @param contacts the contacts + * @param dt the time step + */ + void solve(IRigidbody rigidbody, List contacts, double dt); + + /** + * Computes the restitution coefficient of a collision between two blocks on rigidbodies. + */ + double getRestitution(BlockState blockA, BlockState blockB); +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/resolution/SequentialManifoldSolver.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/resolution/SequentialManifoldSolver.java new file mode 100644 index 0000000..700edfe --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/resolution/SequentialManifoldSolver.java @@ -0,0 +1,105 @@ +package com.eriksonn.createaeronautics.physics.collision.resolution; + +import com.eriksonn.createaeronautics.physics.AbstractContraptionRigidbody; +import com.eriksonn.createaeronautics.physics.IRigidbody; +import com.eriksonn.createaeronautics.physics.SimulatedContraptionRigidbody; +import com.eriksonn.createaeronautics.physics.collision.detection.Contact; +import com.eriksonn.createaeronautics.physics.collision.detection.Manifold; +import com.sun.org.apache.bcel.internal.generic.DCONST; +import hellfirepvp.astralsorcery.common.util.data.Vector3; +import net.minecraft.block.BlockState; +import net.minecraft.util.math.vector.Vector3d; + +import java.util.List; + +import static com.eriksonn.createaeronautics.physics.collision.detection.GJKEPA.EPSILON; +import static java.lang.Math.max; +import static java.lang.Math.min; + +/** + * Manifold solver making use of sequential impulse resolution. + */ +public class SequentialManifoldSolver implements IIterativeManifoldSolver { + + @Override + public double getRestitution(BlockState blockA, BlockState blockB) { + return 0.0; + } + + @Override + public void preSolve(List contacts) { + + // Iterate over all contacts + for (Contact contact : contacts) { + + // Inverse Mass + double inverseMass = 1.0 / contact.rigidbody.getMass(); + contact.inverseMass = inverseMass; + + contact.normalImpulse = 0.0; + contact.tangentImpulse = 0.0; + + contact.localContactPoint = contact.rigidbody.toLocal(contact.manifold.contactPointA); + + Vector3d localNormal = contact.rigidbody.rotateInverse(contact.manifold.normal); + contact.normalMass = inverseMass + contact.rigidbody.multiplyInertiaInverse(contact.localContactPoint.cross(localNormal)).cross(contact.localContactPoint).dot(localNormal); + } + + } + + @Override + public void solve(IRigidbody rigidbody, List contacts, double dt) { + for (Contact contact : contacts) { + Vector3d relativeVelocity = rigidbody.getVelocityAtPoint(contact.localContactPoint); + + // Relative velocity along the collision normal + double velocityAlongCollisionNormal = relativeVelocity.dot(contact.manifold.normal.scale(-1.0)); + + // Total restitution coefficient(commonly denoted as `e`) + double e = getRestitution(contact.blockA, contact.blockB); + + // Impulse scalar + double impulseDelta = (-(1.0 + e) * velocityAlongCollisionNormal) / (contact.normalMass); + + // Impulse clamping based on Erin Catto's GDC 2014 talk + double newImpulse = Math.max(contact.normalImpulse + impulseDelta, 0); + impulseDelta = newImpulse - contact.normalImpulse; + contact.normalImpulse = newImpulse; + + // The impulse vector + Vector3d impulse = contact.manifold.normal.scale(impulseDelta); + + // If the impulse isn't NaN + if (!Double.isNaN(impulse.x)) { + rigidbody.applyImpulse(contact.localContactPoint, impulse.scale(-1.0)); + + relativeVelocity = rigidbody.getVelocityAtPoint(contact.localContactPoint); + + // Friction! + // Get tangent vec + Vector3d tangent = relativeVelocity.subtract(contact.manifold.normal.scale(relativeVelocity.dot(contact.manifold.normal))).normalize(); + + // Friction vector magnitude, denoted as jt + double jt = (-relativeVelocity.dot(tangent)) / contact.inverseMass; + + if (Math.abs(jt) < EPSILON) + return; + + double staticFriction = 0.8, + dynamicFriction = 0.7; + + // Friction impulse + // Coulumb's law + Vector3d tangentImpulse; + if (Math.abs(jt) < impulseDelta * staticFriction) + tangentImpulse = tangent.scale(jt); + else + tangentImpulse = tangent.scale(-impulseDelta).scale(dynamicFriction); + +// rigidbody.applyImpulse(veloPoint, tangentImpulse.scale(-1.0f)); + } + } + } + + +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/CollisionQuality.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/CollisionQuality.java new file mode 100644 index 0000000..1effb86 --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/CollisionQuality.java @@ -0,0 +1,35 @@ +package com.eriksonn.createaeronautics.physics.collision.shape; + +/** + * Represents the quality of collision detection. + * Collision resolution and generation are not affected by this. + */ +public enum CollisionQuality { + /** + * Best quality, use of the GJK and EPA algorithms. + */ + GJKEPA("High (GJK + EPA)"), + + /** + * Spheres are used to more accurately fit the shape of the block. + * This can result in possible penetration, however performance is much better. + */ + SPHERE_AABB("Medium (Sphere AABB)"), + + /** + * Spheres are used to roughly fit the shape of the block. + * This can result in possible penetration up to around 1/3 of a block. + */ + SPHERE("Low (Sphere)"); + + private final String name; + + CollisionQuality(String name) { + this.name = name; + } + + @Override + public String toString() { + return name; + } +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/ICollisionShape.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/ICollisionShape.java new file mode 100644 index 0000000..c5e4962 --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/ICollisionShape.java @@ -0,0 +1,45 @@ +package com.eriksonn.createaeronautics.physics.collision.shape; + +import com.eriksonn.createaeronautics.physics.IRigidbody; +import net.minecraft.util.math.AxisAlignedBB; +import net.minecraft.util.math.vector.Quaternion; +import net.minecraft.util.math.vector.Vector3d; + +/** + * Interface for convex shapes, desgined for use with GJK. + * + * @author RyanHCode + */ +public interface ICollisionShape { + + /** + * Returns the smallest AABB that contains the entire shape. + * + * @return The smallest AABB that contains the entire shape. + */ + AxisAlignedBB getBounds(); + + /** + * Given a direction vector, return the furthest point in that direction. + * + * @param direction The direction vector. + * @return The furthest point in that direction. + */ + Vector3d support(Vector3d direction); + + /** + * Returns the center of the shape. + * + * @return The center of the shape. + */ + Vector3d getCenter(); + + /** + * Sets the transform of the shape, to be applied to every vector. + * + * @param localPos The translation of the shape. + * @param transform The rigidbody to use for transformation + */ + void setTransform(Vector3d localPos, IRigidbody transform); + +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/ICollisionShapeGenerator.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/ICollisionShapeGenerator.java new file mode 100644 index 0000000..fd6fb46 --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/ICollisionShapeGenerator.java @@ -0,0 +1,30 @@ +package com.eriksonn.createaeronautics.physics.collision.shape; + +import com.simibubi.create.content.contraptions.components.structureMovement.AbstractContraptionEntity; +import net.minecraft.block.BlockState; +import net.minecraft.util.math.BlockPos; +import net.minecraft.world.IBlockReader; + +import java.util.HashMap; +import java.util.List; + +/** + * Interface for generating collision shapes from contraptions & blocks. + * + * @author RyanHCode + */ +public interface ICollisionShapeGenerator { + + /** + * Generates collision shapes from a given contraption. + * @param contraption The contraption to generate the collision shape from. + * @return The generated collision shapes. + */ + HashMap> generateShapes(AbstractContraptionEntity contraption); + + /** + * Generates collision shapes given a block + */ + List generateFromBlock(IBlockReader level, BlockPos position, BlockState block, boolean isOnContraption); + +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/MeshCollisionShape.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/MeshCollisionShape.java new file mode 100644 index 0000000..5e5535c --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/MeshCollisionShape.java @@ -0,0 +1,83 @@ +package com.eriksonn.createaeronautics.physics.collision.shape; + +import com.eriksonn.createaeronautics.physics.IRigidbody; +import net.minecraft.util.math.AxisAlignedBB; +import net.minecraft.util.math.vector.Vector3d; + +public class MeshCollisionShape implements ICollisionShape { + + public Vector3d[] vertices; + + public MeshCollisionShape(Vector3d[] vertices) { + this.vertices = vertices; + } + + @Override + public AxisAlignedBB getBounds() { + // Find the min and max values for each axis + double minX = Float.MAX_VALUE; + double minY = Float.MAX_VALUE; + double minZ = Float.MAX_VALUE; + double maxX = -Float.MAX_VALUE; + double maxY = -Float.MAX_VALUE; + double maxZ = -Float.MAX_VALUE; + + for(Vector3d unProcessed : vertices){ + Vector3d v = applyTransform(unProcessed); + if(v.x < minX) minX = v.x; + if(v.y < minY) minY = v.y; + if(v.z < minZ) minZ = v.z; + if(v.x > maxX) maxX = v.x; + if(v.y > maxY) maxY = v.y; + if(v.z > maxZ) maxZ = v.z; + } + + return new AxisAlignedBB(minX, minY, minZ, maxX, maxY, maxZ); + } + + @Override + public Vector3d support(Vector3d direction) { + + // find the vertex with the largest dot product with the direction + double maxDot = -Float.MAX_VALUE; + + Vector3d maxVertex = null; + for(Vector3d raw : vertices){ + Vector3d v = applyTransform(raw); + double dot = v.subtract(getCenter()).dot(direction); + if(dot > maxDot) { + maxDot = dot; + maxVertex = v; + } + } + + return maxVertex; + } + + @Override + public Vector3d getCenter() { + // Average all vertex positions + Vector3d sum = new Vector3d(0, 0, 0); + + for(Vector3d v : vertices){ + sum = sum.add(applyTransform(v)); + } + + return sum.scale(1.0 / vertices.length); + } + + // Current translation & rotation transform + private Vector3d localPos = new Vector3d(0, 0, 0); + private IRigidbody transform = null; + + public Vector3d applyTransform(Vector3d vec) { + if(transform == null) return vec.add(this.localPos); + return transform.toGlobal(vec.add(this.localPos)); + } + + @Override + public void setTransform(Vector3d localPos, IRigidbody transform) { + this.localPos = localPos; + this.transform = transform; + } +} diff --git a/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/MeshCollisionShapeGenerator.java b/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/MeshCollisionShapeGenerator.java new file mode 100644 index 0000000..65ae8db --- /dev/null +++ b/src/main/java/com/eriksonn/createaeronautics/physics/collision/shape/MeshCollisionShapeGenerator.java @@ -0,0 +1,94 @@ +package com.eriksonn.createaeronautics.physics.collision.shape; + +import com.eriksonn.createaeronautics.physics.AbstractContraptionRigidbody; +import com.simibubi.create.content.contraptions.components.structureMovement.AbstractContraptionEntity; +import net.minecraft.block.BlockState; +import net.minecraft.block.Blocks; +import net.minecraft.util.math.AxisAlignedBB; +import net.minecraft.util.math.BlockPos; +import net.minecraft.util.math.shapes.VoxelShape; +import net.minecraft.util.math.vector.Vector3d; +import net.minecraft.world.IBlockReader; +import net.minecraft.world.gen.feature.template.Template; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +public class MeshCollisionShapeGenerator implements ICollisionShapeGenerator { + + AbstractContraptionRigidbody rigidbody; + + public MeshCollisionShapeGenerator(AbstractContraptionRigidbody rigidbody) { + this.rigidbody = rigidbody; + } + + /** + * Computes the vertices of a given AABB + */ + public static Vector3d[] vertices(AxisAlignedBB box) { + return new Vector3d[]{ + new Vector3d(box.minX, box.minY, box.minZ), + new Vector3d(box.maxX, box.minY, box.minZ), + new Vector3d(box.maxX, box.minY, box.maxZ), + new Vector3d(box.minX, box.minY, box.maxZ), + + new Vector3d(box.minX, box.maxY, box.minZ), + new Vector3d(box.maxX, box.maxY, box.minZ), + new Vector3d(box.maxX, box.maxY, box.maxZ), + new Vector3d(box.minX, box.maxY, box.maxZ) + }; + } + + @Override + public HashMap> generateShapes(AbstractContraptionEntity contraption) { + // for every block + Map blocks = contraption.getContraption().getBlocks(); + + HashMap> shapes = new HashMap<>(); + + for (Map.Entry entry : blocks.entrySet()) { + BlockPos pos = entry.getKey(); + BlockState block = entry.getValue().state; + + if( + blocks.containsKey(pos.above()) && blocks.get(pos.above()).state.getBlock().defaultBlockState() == Blocks.AIR.defaultBlockState() + && blocks.containsKey(pos.below()) && blocks.get(pos.below()).state.getBlock().defaultBlockState() == Blocks.AIR.defaultBlockState() + && blocks.containsKey(pos.north()) && blocks.get(pos.north()).state.getBlock().defaultBlockState() == Blocks.AIR.defaultBlockState() + && blocks.containsKey(pos.south()) && blocks.get(pos.south()).state.getBlock().defaultBlockState() == Blocks.AIR.defaultBlockState() + && blocks.containsKey(pos.east()) && blocks.get(pos.east()).state.getBlock().defaultBlockState() == Blocks.AIR.defaultBlockState() + && blocks.containsKey(pos.west()) && blocks.get(pos.west()).state.getBlock().defaultBlockState() == Blocks.AIR.defaultBlockState() + ) { + continue; + } + + // generate collision shapes + List collisionShapes = generateFromBlock(contraption.getContraption().getContraptionWorld(), pos, block, true); + shapes.put(pos, collisionShapes); + } + + return shapes; + } + + public List generateFromBlock(IBlockReader level, BlockPos position, BlockState block, boolean isOnContraption) { + VoxelShape voxelShape = block.getCollisionShape(level, position); + + List shapes = new ArrayList<>(); + + voxelShape.forAllBoxes((minX, minY, minZ, maxX, maxY, maxZ) -> { + Vector3d[] vertices = vertices(new AxisAlignedBB(minX, minY, minZ, maxX, maxY, maxZ)); + + // mesh collision shape + MeshCollisionShape shape = new MeshCollisionShape(vertices); + if(isOnContraption) + shape.setTransform(new Vector3d(position.getX(), position.getY(), position.getZ()), rigidbody); + else + shape.setTransform(new Vector3d(position.getX(), position.getY(), position.getZ()), null); + shapes.add(shape); + }); + + return shapes; + } + +}