From 70bc473277fed4701cb65e504b99d1a485ce5bcb Mon Sep 17 00:00:00 2001 From: austin Date: Fri, 24 Jan 2025 16:19:04 -0500 Subject: [PATCH] floating origin for CollisionEngine.java --- docs/src/progress/renderertodo.md | 2 + .../collision/CollisionBodyCreation.java | 2 +- .../collision/CollisionEngine.java | 65 ++++++++++++++++--- .../electrosphere/collision/PhysicsUtils.java | 9 --- .../entity/state/attach/AttachUtils.java | 37 ++++++++++- .../state/hitbox/HitboxCollectionState.java | 11 ++-- .../datacell/GriddedDataCellManager.java | 10 +-- .../electrosphere/server/datacell/Realm.java | 4 ++ 8 files changed, 110 insertions(+), 30 deletions(-) diff --git a/docs/src/progress/renderertodo.md b/docs/src/progress/renderertodo.md index b2321ca0..1198230e 100644 --- a/docs/src/progress/renderertodo.md +++ b/docs/src/progress/renderertodo.md @@ -1301,6 +1301,8 @@ Save assembly on compile Delete old server physics cell on terrain edit Fix server terrain physics entity positioning Disable client fluid draw cell loading gate +Properly differentiate local/world bone attach point calculation +Floating origin implementation for collision engine diff --git a/src/main/java/electrosphere/collision/CollisionBodyCreation.java b/src/main/java/electrosphere/collision/CollisionBodyCreation.java index 3db5413a..df6e7da2 100644 --- a/src/main/java/electrosphere/collision/CollisionBodyCreation.java +++ b/src/main/java/electrosphere/collision/CollisionBodyCreation.java @@ -212,7 +212,7 @@ public class CollisionBodyCreation { } /** - * Sets the offset position of the first geometry in a given body + * Sets the offset position of the first geometry in a given body (local to the parent) * @param collisionEngine The collision engine * @param body The body * @param offsetPosition The position to offset the first geometry by diff --git a/src/main/java/electrosphere/collision/CollisionEngine.java b/src/main/java/electrosphere/collision/CollisionEngine.java index c42f0701..af311902 100644 --- a/src/main/java/electrosphere/collision/CollisionEngine.java +++ b/src/main/java/electrosphere/collision/CollisionEngine.java @@ -116,6 +116,13 @@ public class CollisionEngine { //The list of all collidables the engine is currently tracking List collidableList = new ArrayList(); + /** + * Dynamic spatial offset applied to all operations on the space. + * This is used for world origin rebasing. + * This makes physics behave more consistently by moving far out objects to center around 0,0,0. + */ + Vector3d floatingOrigin = new Vector3d(0,0,0); + //callbacks for collision check RayCastCallback rayCastCallback = new RayCastCallback(); @@ -530,7 +537,7 @@ public class CollisionEngine { Entity physicsEntity = collidable.getParent(); DBody rigidBody = PhysicsEntityUtils.getDBody(physicsEntity); Matrix4d inverseTransform = new Matrix4d(); - Vector4d rawPos = inverseTransform.transform(new Vector4d(PhysicsUtils.getRigidBodyPosition(rigidBody),1)); + Vector4d rawPos = inverseTransform.transform(new Vector4d(PhysicsUtils.odeVecToJomlVec(rigidBody.getPosition()).add(this.floatingOrigin),1)); Vector3d calculatedPosition = new Vector3d(rawPos.x,rawPos.y,rawPos.z); Vector3d suggestedPosition = this.suggestMovementPosition(collisionWorldData, calculatedPosition); if(calculatedPosition.distance(suggestedPosition) > 0){ @@ -545,6 +552,48 @@ public class CollisionEngine { spaceLock.unlock(); Globals.profiler.endCpuSample(); } + + /** + * Rebases the world origin + */ + public void rebaseWorldOrigin(){ + Globals.profiler.beginCpuSample("rebaseWorldOrigin"); + spaceLock.lock(); + if(this.collisionWorldData != null && this.floatingOrigin.x == 0 && this.floatingOrigin.z == 0){ + int collected = 0; + Vector3d newOrigin = new Vector3d(); + //calculate new origin + for(Collidable collidable : collidableList){ + Entity physicsEntity = collidable.getParent(); + DBody rigidBody = PhysicsEntityUtils.getDBody(physicsEntity); + if(rigidBody != null){ + Vector3d currentBodyOffset = PhysicsUtils.odeVecToJomlVec(rigidBody.getPosition()); + if(collected == 0){ + newOrigin.set(currentBodyOffset); + } else { + float percentExisting = collected / (float)(collected + 1); + float percentNew = 1.0f - percentExisting; + newOrigin = newOrigin.mul(percentExisting).add(currentBodyOffset.mul(percentNew)); + } + collected++; + } + } + Vector3d delta = new Vector3d(this.floatingOrigin).sub(newOrigin); + this.floatingOrigin = this.floatingOrigin.set(newOrigin); + //apply new origin to all geoms + //calculate new origin + for(Collidable collidable : collidableList){ + Entity physicsEntity = collidable.getParent(); + DBody rigidBody = PhysicsEntityUtils.getDBody(physicsEntity); + if(rigidBody != null){ + Vector3d existingPosition = PhysicsUtils.odeVecToJomlVec(rigidBody.getPosition()); + rigidBody.setPosition(PhysicsUtils.jomlVecToOdeVec(existingPosition.add(delta))); + } + } + } + spaceLock.unlock(); + Globals.profiler.endCpuSample(); + } /** * Registers a collision object with the server @@ -573,7 +622,7 @@ public class CollisionEngine { public void listBodyPositions(){ for(DBody body : bodies){ LoggerInterface.loggerEngine.INFO("" + body); - LoggerInterface.loggerEngine.INFO("" + PhysicsUtils.getRigidBodyPosition(body)); + LoggerInterface.loggerEngine.INFO("" + PhysicsUtils.odeVecToJomlVec(body.getPosition()).add(this.floatingOrigin)); } } @@ -914,7 +963,7 @@ public class CollisionEngine { */ protected void setBodyTransform(DBody body, Vector3d position, Quaterniond rotation){ spaceLock.lock(); - body.setPosition(position.x, position.y, position.z); + body.setPosition(position.x - this.floatingOrigin.x, position.y - this.floatingOrigin.y, position.z - this.floatingOrigin.z); body.setQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation)); spaceLock.unlock(); } @@ -932,7 +981,7 @@ public class CollisionEngine { protected void synchronizeData(DBody body, Vector3d position, Quaterniond rotation, Vector3d linearVel, Vector3d angularVel, Vector3d linearForce, Vector3d angularForce){ if(body != null){ spaceLock.lock(); - body.setPosition(position.x, position.y, position.z); + body.setPosition(position.x - this.floatingOrigin.x, position.y - this.floatingOrigin.y, position.z - this.floatingOrigin.z); body.setQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation)); body.setLinearVel(PhysicsUtils.jomlVecToOdeVec(linearVel)); body.setAngularVel(PhysicsUtils.jomlVecToOdeVec(angularVel)); @@ -951,7 +1000,7 @@ public class CollisionEngine { */ protected void setBodyTransform(DBody body, CollidableTemplate template, Vector3d position, Quaterniond rotation, Vector3d scale){ spaceLock.lock(); - body.setPosition(position.x, position.y, position.z); + body.setPosition(position.x - this.floatingOrigin.x, position.y - this.floatingOrigin.y, position.z - this.floatingOrigin.z); body.setQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation)); DGeom firstGeom = body.getFirstGeom(); if(firstGeom instanceof DCylinder){ @@ -965,14 +1014,14 @@ public class CollisionEngine { } /** - * Sets the transform of a geometry + * Sets the transform of a geometry (local to the parent) * @param geom The geometry * @param position The position * @param rotation The rotation */ protected void setGeomTransform(DGeom geom, Vector3d position, Quaterniond rotation){ spaceLock.lock(); - geom.setOffsetWorldPosition(position.x, position.y, position.z); + geom.setOffsetPosition(position.x, position.y, position.z); geom.setOffsetQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation)); spaceLock.unlock(); } @@ -995,7 +1044,7 @@ public class CollisionEngine { protected Vector3d getBodyPosition(DBody body){ Vector3d rVal = null; spaceLock.lock(); - rVal = PhysicsUtils.odeVecToJomlVec(body.getPosition()); + rVal = PhysicsUtils.odeVecToJomlVec(body.getPosition()).add(this.floatingOrigin); spaceLock.unlock(); return rVal; } diff --git a/src/main/java/electrosphere/collision/PhysicsUtils.java b/src/main/java/electrosphere/collision/PhysicsUtils.java index 38ec032f..7aeb8793 100644 --- a/src/main/java/electrosphere/collision/PhysicsUtils.java +++ b/src/main/java/electrosphere/collision/PhysicsUtils.java @@ -15,15 +15,6 @@ import electrosphere.game.data.collidable.CollidableTemplate; */ public class PhysicsUtils { - /** - * Gets the current position of a rigid body as a joml vector - * @param body The dbody - * @return The position - */ - public static Vector3d getRigidBodyPosition(DBody body){ - return odeVecToJomlVec(body.getPosition()); - } - /** * Gets the rotation of an ode body as a joml quaternion * @param body diff --git a/src/main/java/electrosphere/entity/state/attach/AttachUtils.java b/src/main/java/electrosphere/entity/state/attach/AttachUtils.java index f15d0d6e..98d032e6 100644 --- a/src/main/java/electrosphere/entity/state/attach/AttachUtils.java +++ b/src/main/java/electrosphere/entity/state/attach/AttachUtils.java @@ -294,7 +294,7 @@ public class AttachUtils { Vector3d parentScale ){ //transform bone space - Vector3d position = AttachUtils.calculateBoneAttachmentPosition( + Vector3d position = AttachUtils.calculateBoneAttachmentWorldPosition( offsetVector, offsetRotation, bonePosition, @@ -331,7 +331,7 @@ public class AttachUtils { * @param parentScale The parent's scale * @return The position of the attached/child entity */ - public static Vector3d calculateBoneAttachmentPosition( + public static Vector3d calculateBoneAttachmentLocalPosition( //optional offsets Vector3d offsetVector, Quaterniond offsetRotation, @@ -352,6 +352,39 @@ public class AttachUtils { position = position.mul(parentScale); position = position.rotate(new Quaterniond(parentRotation)); //transform worldspace + // position.add(parentPosition); + + return position; + } + + /** + * Calculates the position of an entity attached to a bone + * @param offsetVector The offset position + * @param offsetRotation The offset rotation + * @param bonePosition The bone's position + * @param boneRotation The bone's rotation + * @param parentPosition The parent's position + * @param parentRotation The parent's rotation + * @param parentScale The parent's scale + * @return The position of the attached/child entity + */ + public static Vector3d calculateBoneAttachmentWorldPosition( + //optional offsets + Vector3d offsetVector, + Quaterniond offsetRotation, + + //current bone transform + Vector3d bonePosition, + Quaterniond boneRotation, + + //parent transforms + Vector3d parentPosition, + Quaterniond parentRotation, + Vector3d parentScale + ){ + //transform bone space + Vector3d position = calculateBoneAttachmentLocalPosition(offsetVector, offsetRotation, bonePosition, boneRotation, parentPosition, parentRotation, parentScale); + //transform worldspace position.add(parentPosition); return position; diff --git a/src/main/java/electrosphere/entity/state/hitbox/HitboxCollectionState.java b/src/main/java/electrosphere/entity/state/hitbox/HitboxCollectionState.java index 9660d293..fb45f157 100644 --- a/src/main/java/electrosphere/entity/state/hitbox/HitboxCollectionState.java +++ b/src/main/java/electrosphere/entity/state/hitbox/HitboxCollectionState.java @@ -253,7 +253,7 @@ public class HitboxCollectionState { if(parent != null && !isServer && EntityUtils.getActor(parent) != null){ if(!this.geoms.isEmpty()){ Vector3d entityPosition = EntityUtils.getPosition(parent); - this.body.setPosition(PhysicsUtils.jomlVecToOdeVec(entityPosition)); + PhysicsUtils.setRigidBodyTransform(collisionEngine, entityPosition, new Quaterniond(), body); for(String boneName : this.boneHitboxMap.keySet()){ if(EntityUtils.getActor(parent).containsBone(boneName)){ Vector3d bonePosition = EntityUtils.getActor(parent).getBonePosition(boneName); @@ -293,7 +293,7 @@ public class HitboxCollectionState { } else if(parent != null && isServer && EntityUtils.getPoseActor(parent) != null){ if(!this.geoms.isEmpty()){ Vector3d entityPosition = EntityUtils.getPosition(parent); - this.body.setPosition(PhysicsUtils.jomlVecToOdeVec(entityPosition)); + PhysicsUtils.setRigidBodyTransform(collisionEngine, entityPosition, new Quaterniond(), body); // for(String boneName : this.boneHitboxMap.keySet()){ if(EntityUtils.getPoseActor(parent).containsBone(boneName)){ @@ -358,8 +358,7 @@ public class HitboxCollectionState { * @param bonePosition The position of the bone */ private void updateStaticCapsulePosition(CollisionEngine collisionEngine, DGeom geom, HitboxState shapeStatus){ - Vector3d parentPos = EntityUtils.getPosition(parent); - PhysicsEntityUtils.setGeometryPosition(collisionEngine, geom, parentPos, new Quaterniond(0.707,0,0,0.707)); + PhysicsEntityUtils.setGeometryPosition(collisionEngine, geom, new Vector3d(), new Quaterniond(0.707,0,0,0.707)); } /** @@ -388,7 +387,7 @@ public class HitboxCollectionState { Vector3d parentScale = new Vector3d(EntityUtils.getScale(parent)); //calculate - Vector3d hitboxPos = AttachUtils.calculateBoneAttachmentPosition(offsetPosition, offsetRotation, bonePositionD, boneRotation, parentPosition, parentRotation, parentScale); + Vector3d hitboxPos = AttachUtils.calculateBoneAttachmentLocalPosition(offsetPosition, offsetRotation, bonePositionD, boneRotation, parentPosition, parentRotation, parentScale); PhysicsEntityUtils.setGeometryPosition(collisionEngine, geom, hitboxPos, new Quaterniond()); @@ -425,7 +424,7 @@ public class HitboxCollectionState { Vector3d parentScale = new Vector3d(EntityUtils.getScale(parent)); //calculate - Vector3d worldPosition = AttachUtils.calculateBoneAttachmentPosition(offsetPosition, offsetRotation, bonePositionD, boneRotation, parentPosition, parentRotation, parentScale); + Vector3d worldPosition = AttachUtils.calculateBoneAttachmentLocalPosition(offsetPosition, offsetRotation, bonePositionD, boneRotation, parentPosition, parentRotation, parentScale); Quaterniond worldRotation = new Quaterniond(); if(previousWorldPos != null){ diff --git a/src/main/java/electrosphere/server/datacell/GriddedDataCellManager.java b/src/main/java/electrosphere/server/datacell/GriddedDataCellManager.java index 7433438f..745e600c 100644 --- a/src/main/java/electrosphere/server/datacell/GriddedDataCellManager.java +++ b/src/main/java/electrosphere/server/datacell/GriddedDataCellManager.java @@ -537,10 +537,12 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager cellPlayerlessFrameMap.put(groundDataCells.get(this.getServerDataCellKey(worldPos)),0); loadedCellsLock.unlock(); } else if(groundDataCells.get(this.getServerDataCellKey(worldPos)) == null) { - LoggerInterface.loggerEngine.WARNING( - "Trying to create data cell outside world bounds!\n" + - worldPos + "\n" + - this.serverWorldData.getWorldSizeDiscrete() + LoggerInterface.loggerEngine.ERROR( + new Error( + "Trying to create data cell outside world bounds!\n" + + worldPos + "\n" + + this.serverWorldData.getWorldSizeDiscrete() + ) ); } return groundDataCells.get(this.getServerDataCellKey(worldPos)); diff --git a/src/main/java/electrosphere/server/datacell/Realm.java b/src/main/java/electrosphere/server/datacell/Realm.java index 3da21c5f..21281c9e 100644 --- a/src/main/java/electrosphere/server/datacell/Realm.java +++ b/src/main/java/electrosphere/server/datacell/Realm.java @@ -218,6 +218,10 @@ public class Realm { collisionEngine.clearCollidableImpulseLists(); chemistryEngine.clearCollidableImpulseLists(); + // + //rebase physics origin + this.collisionEngine.rebaseWorldOrigin(); + Globals.profiler.endCpuSample(); }