diff --git a/src/main/java/org/terasology/minecarts/Util.java b/src/main/java/org/terasology/minecarts/Util.java index ad83c45..7e58124 100644 --- a/src/main/java/org/terasology/minecarts/Util.java +++ b/src/main/java/org/terasology/minecarts/Util.java @@ -17,8 +17,34 @@ import org.joml.Vector3f; import org.joml.Vector3fc; +import org.terasology.logic.location.LocationComponent; +import org.terasology.math.JomlUtil; public class Util { + public static void bound(Vector3f v) { + if (Float.isNaN(v.x) || Float.isInfinite(v.x)) + v.x = 0.0f; + if (Float.isNaN(v.y) || Float.isInfinite(v.y)) + v.y = 0.0f; + if (Float.isNaN(v.z) || Float.isInfinite(v.z)) + v.z = 0.0f; + } + + public static Vector3f localToWorldPosition(Vector3f localPosition, LocationComponent locationComponent) { + Vector3f worldPosition = new Vector3f(localPosition); + + LocationComponent parentLocation = locationComponent; + while (parentLocation != null) { + worldPosition.mul(parentLocation.getLocalScale()); + parentLocation.getLocalRotation().rotate(JomlUtil.from(worldPosition), JomlUtil.from(worldPosition)); + Vector3f parentLocalPosition = new Vector3f(JomlUtil.from(parentLocation.getLocalPosition())); + worldPosition.add(parentLocalPosition); + parentLocation = parentLocation.getParent().getComponent(LocationComponent.class); + } + + return worldPosition; + } + public static Vector3f project(Vector3fc u, Vector3fc v, Vector3f dest) { return dest.set(v).mul(u.dot(v) / v.lengthSquared()); } diff --git a/src/main/java/org/terasology/minecarts/controllers/CartImpulseSystem.java b/src/main/java/org/terasology/minecarts/controllers/CartImpulseSystem.java index 58c4c2a..969c967 100644 --- a/src/main/java/org/terasology/minecarts/controllers/CartImpulseSystem.java +++ b/src/main/java/org/terasology/minecarts/controllers/CartImpulseSystem.java @@ -17,7 +17,6 @@ import org.terasology.logic.location.LocationComponent; import org.terasology.math.JomlUtil; import org.terasology.minecarts.Constants; -import org.terasology.minecarts.Util; import org.terasology.minecarts.components.CartJointComponent; import org.terasology.minecarts.components.CollisionFilterComponent; import org.terasology.minecarts.components.RailVehicleComponent; @@ -57,7 +56,7 @@ public static void removeCollisionFilter(EntityRef cart, EntityRef child) { @ReceiveEvent(components = {RailVehicleComponent.class, PathFollowerComponent.class, LocationComponent.class, - RigidBodyComponent.class}, priority = EventPriority.PRIORITY_HIGH) + RigidBodyComponent.class}, priority = EventPriority.PRIORITY_HIGH) public void onBump(CollideEvent event, EntityRef entity) { CollisionFilterComponent collisionFilterComponent = entity.getComponent(CollisionFilterComponent.class); if (collisionFilterComponent != null && collisionFilterComponent.filter.contains(event.getOtherEntity())) { @@ -102,10 +101,11 @@ private void handleCharacterCollision(CollideEvent event, EntityRef entity) { float effectiveMass = (1.0f / r1.mass) + (1.0f / Constants.PLAYER_MASS); Vector3f df = - new Vector3f(JomlUtil.from(v2l.getWorldPosition())).sub(JomlUtil.from(v1l.getWorldPosition())).normalize(); + new Vector3f(v2l.getWorldPosition(new Vector3f())).sub(v1l.getWorldPosition(new Vector3f())).normalize(); float b = - -df.dot(event.getNormal()) * (Constants.BAUMGARTE_COFF / time.getGameDelta()) * event.getPenetration(); + -df.dot(event.getNormal()) * (Constants.BAUMGARTE_COFF / time.getGameDelta()) * event.getPenetration(); + float lambda = -(jv + b) / effectiveMass; @@ -114,15 +114,15 @@ private void handleCharacterCollision(CollideEvent event, EntityRef entity) { } Vector3f r1v = new Vector3f( - event.getNormal().x / r1.mass, - event.getNormal().y / r1.mass, - event.getNormal().z / r1.mass) - .mul(lambda); + event.getNormal().x / r1.mass, + event.getNormal().y / r1.mass, + event.getNormal().z / r1.mass) + .mul(lambda); Vector3f r2v = new Vector3f( - event.getNormal().x / Constants.PLAYER_MASS, - event.getNormal().y / Constants.PLAYER_MASS, - event.getNormal().z / Constants.PLAYER_MASS) - .mul(lambda).mul(-1); + event.getNormal().x / Constants.PLAYER_MASS, + event.getNormal().y / Constants.PLAYER_MASS, + event.getNormal().z / Constants.PLAYER_MASS) + .mul(lambda).mul(-1); v1.velocity.add(r1v); event.getOtherEntity().send(new CharacterImpulseEvent(JomlUtil.from(r2v))); @@ -142,9 +142,9 @@ private void handleCartCollision(CollideEvent event, EntityRef entity) { LocationComponent v2l = event.getOtherEntity().getComponent(LocationComponent.class); - Vector3f df = new Vector3f(JomlUtil.from(v2l.getWorldPosition())) - .sub(JomlUtil.from(v1l.getWorldPosition())) - .add(new Vector3f(Float.MIN_VALUE, Float.MIN_VALUE, Float.MIN_VALUE)).normalize(); + Vector3f df = new Vector3f(v2l.getWorldPosition(new Vector3f())) + .sub(v1l.getWorldPosition(new Vector3f())) + .add(new Vector3f(Float.MIN_VALUE, Float.MIN_VALUE, Float.MIN_VALUE)).normalize(); //calculate the half normal vector Vector3f normal = new Vector3f(df); diff --git a/src/main/java/org/terasology/minecarts/controllers/CartJointSystem.java b/src/main/java/org/terasology/minecarts/controllers/CartJointSystem.java index 1915468..1597d04 100644 --- a/src/main/java/org/terasology/minecarts/controllers/CartJointSystem.java +++ b/src/main/java/org/terasology/minecarts/controllers/CartJointSystem.java @@ -3,6 +3,7 @@ package org.terasology.minecarts.controllers; +import org.joml.Quaternionf; import org.joml.Vector3f; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -13,7 +14,6 @@ import org.terasology.entitySystem.systems.RegisterSystem; import org.terasology.entitySystem.systems.UpdateSubscriberSystem; import org.terasology.logic.location.LocationComponent; -import org.terasology.math.JomlUtil; import org.terasology.minecarts.Constants; import org.terasology.minecarts.Util; import org.terasology.minecarts.components.CartJointComponent; @@ -25,7 +25,7 @@ @RegisterSystem(RegisterMode.AUTHORITY) @Share(CartJointSystem.class) -public class CartJointSystem extends BaseComponentSystem implements UpdateSubscriberSystem { +public class CartJointSystem extends BaseComponentSystem implements UpdateSubscriberSystem { private static final Logger LOGGER = LoggerFactory.getLogger(CartJointSystem.class); @In @@ -45,16 +45,20 @@ public boolean joinVehicles(EntityRef entity1, EntityRef entity2) { boolean isJoined = false; if (cartJointComponent1.back != null && cartJointComponent2.back != null) { - isJoined = tryJoin(new Vector3f(0, 0, -1), entity1, cartJointComponent1.back, new Vector3f(0, 0, -1), entity2, cartJointComponent2.back); + isJoined = tryJoin(new Vector3f(0, 0, -1), entity1, cartJointComponent1.back, new Vector3f(0, 0, -1), + entity2, cartJointComponent2.back); } if (!isJoined && cartJointComponent1.back != null && cartJointComponent2.front != null) { - isJoined = tryJoin(new Vector3f(0, 0, -1), entity1, cartJointComponent1.back, new Vector3f(0, 0, 1), entity2, cartJointComponent2.front); + isJoined = tryJoin(new Vector3f(0, 0, -1), entity1, cartJointComponent1.back, new Vector3f(0, 0, 1), + entity2, cartJointComponent2.front); } if (!isJoined && cartJointComponent1.front != null && cartJointComponent2.back != null) { - isJoined = tryJoin(new Vector3f(0, 0, 1), entity1, cartJointComponent1.front, new Vector3f(0, 0, -1), entity2, cartJointComponent2.back); + isJoined = tryJoin(new Vector3f(0, 0, 1), entity1, cartJointComponent1.front, new Vector3f(0, 0, -1), + entity2, cartJointComponent2.back); } if (!isJoined && cartJointComponent1.front != null && cartJointComponent2.front != null) { - isJoined = tryJoin(new Vector3f(0, 0, 1), entity1, cartJointComponent1.front, new Vector3f(0, 0, 1), entity2, cartJointComponent2.front); + isJoined = tryJoin(new Vector3f(0, 0, 1), entity1, cartJointComponent1.front, new Vector3f(0, 0, 1), + entity2, cartJointComponent2.front); } if (isJoined) { @@ -66,14 +70,15 @@ public boolean joinVehicles(EntityRef entity1, EntityRef entity2) { return isJoined; } - private boolean tryJoin(Vector3f d1, EntityRef e1, CartJointComponent.CartJointSocket j1, Vector3f d2, EntityRef e2, CartJointComponent.CartJointSocket j2) { + private boolean tryJoin(Vector3f d1, EntityRef e1, CartJointComponent.CartJointSocket j1, Vector3f d2, + EntityRef e2, CartJointComponent.CartJointSocket j2) { LocationComponent l1 = e1.getComponent(LocationComponent.class); LocationComponent l2 = e2.getComponent(LocationComponent.class); - Vector3f cart1Direction = JomlUtil.from(l1.getWorldRotation()).transform(new Vector3f(d1)); - Vector3f cart2Direction = JomlUtil.from(l2.getWorldRotation()).transform(new Vector3f(d2)); + Vector3f cart1Direction = l1.getWorldRotation(new Quaternionf()).transform(d1); + Vector3f cart2Direction = l2.getWorldRotation(new Quaternionf()).transform(d2); if (cart1Direction.dot(cart2Direction) < 0) { - if (l1.getWorldPosition().distanceSquared(l2.getWorldPosition()) < (j1.range + j2.range) * (j1.range + j2.range)) { + if (l1.getWorldPosition(new Vector3f()).distanceSquared(l2.getWorldPosition(new Vector3f())) < (j1.range + j2.range) * (j1.range + j2.range)) { j1.entity = e2; j1.isOwning = true; j2.entity = e1; @@ -86,10 +91,12 @@ private boolean tryJoin(Vector3f d1, EntityRef e1, CartJointComponent.CartJointS @Override public void update(float delta) { - for (EntityRef railVehicle : entityManager.getEntitiesWith(RailVehicleComponent.class, RigidBodyComponent.class, CartJointComponent.class)) { + for (EntityRef railVehicle : entityManager.getEntitiesWith(RailVehicleComponent.class, + RigidBodyComponent.class, CartJointComponent.class)) { CartJointComponent cartJointComponent = railVehicle.getComponent(CartJointComponent.class); if (cartJointComponent.front != null && cartJointComponent.front.isOwning) { - CartJointComponent frontCartJoint = cartJointComponent.front.entity.getComponent(CartJointComponent.class); + CartJointComponent frontCartJoint = + cartJointComponent.front.entity.getComponent(CartJointComponent.class); if (frontCartJoint != null) { applyImpulseOnSocket(delta, cartJointComponent.front, frontCartJoint.findJoint(railVehicle)); } @@ -109,7 +116,8 @@ private void clearJoinSocket(CartJointComponent.CartJointSocket jointSocket) { jointSocket.isOwning = false; } - private void applyImpulseOnSocket(float delta, CartJointComponent.CartJointSocket j1, CartJointComponent.CartJointSocket j2) { + private void applyImpulseOnSocket(float delta, CartJointComponent.CartJointSocket j1, + CartJointComponent.CartJointSocket j2) { if (j1.entity == null || j2.entity == null) { return; } @@ -133,7 +141,8 @@ private void applyImpulseOnSocket(float delta, CartJointComponent.CartJointSocke RigidBodyComponent rigidBody = j2.entity.getComponent(RigidBodyComponent.class); RigidBodyComponent otherRigidBody = j1.entity.getComponent(RigidBodyComponent.class); - Vector3f normal = new Vector3f(JomlUtil.from(location.getWorldPosition())).sub(JomlUtil.from(otherLocation.getWorldPosition())); + Vector3f normal = + new Vector3f(location.getWorldPosition(new Vector3f())).sub(otherLocation.getWorldPosition(new Vector3f())); float distance = normal.length(); if (distance > Constants.CART_JOINT_BREAK_DISTANCE) { clearJoinSocket(j1); @@ -142,17 +151,19 @@ private void applyImpulseOnSocket(float delta, CartJointComponent.CartJointSocke return; } - Vector3f projectedNormal = Util.project(segmentVehicle.heading, normal, new Vector3f()).normalize();//segmentVehicle.heading.project(normal).normalize(); + Vector3f projectedNormal = Util.project(segmentVehicle.heading, normal, new Vector3f()).normalize(); + //segmentVehicle.heading.project(normal).normalize(); Vector3f otherProjectedNormal = Util.project(otherSegmentVehicle.heading, normal, new Vector3f()).normalize(); - float relVelAlongNormal = otherRailVehicle.velocity.dot(otherProjectedNormal) - railVehicle.velocity.dot(projectedNormal); + float relVelAlongNormal = + otherRailVehicle.velocity.dot(otherProjectedNormal) - railVehicle.velocity.dot(projectedNormal); float inverseMassSum = 1 / rigidBody.mass + 1 / otherRigidBody.mass; float bias = (Constants.BAUMGARTE_COFF / delta) * ((j1.range + j2.range) - distance); float j = -(relVelAlongNormal + bias) / inverseMassSum; - railVehicle.velocity.sub(new Vector3f(projectedNormal).mul(j / rigidBody.mass)); - otherRailVehicle.velocity.add(new Vector3f(otherProjectedNormal).mul(j / otherRigidBody.mass)); + railVehicle.velocity.sub(projectedNormal.mul(j / rigidBody.mass)); + otherRailVehicle.velocity.add(otherProjectedNormal.mul(j / otherRigidBody.mass)); if (!railVehicle.velocity.isFinite()) { railVehicle.velocity.set(0); diff --git a/src/main/java/org/terasology/minecarts/controllers/CartMotionSystem.java b/src/main/java/org/terasology/minecarts/controllers/CartMotionSystem.java index 701acfb..bcbb45d 100644 --- a/src/main/java/org/terasology/minecarts/controllers/CartMotionSystem.java +++ b/src/main/java/org/terasology/minecarts/controllers/CartMotionSystem.java @@ -18,7 +18,6 @@ import org.terasology.logic.inventory.InventoryManager; import org.terasology.logic.location.LocationComponent; import org.terasology.logic.players.LocalPlayer; -import org.terasology.math.JomlUtil; import org.terasology.minecarts.Constants; import org.terasology.minecarts.Util; import org.terasology.minecarts.blocks.RailBlockSegmentMapper; @@ -73,13 +72,15 @@ public class CartMotionSystem extends BaseComponentSystem implements UpdateSubsc @Override public void initialise() { - segmentMapping = new RailBlockSegmentMapper(blockEntityRegistry, pathFollowerSystem, segmentSystem, segmentCacheSystem); + segmentMapping = new RailBlockSegmentMapper(blockEntityRegistry, pathFollowerSystem, segmentSystem, + segmentCacheSystem); } @Override public void update(float delta) { - for (EntityRef railVehicle : entityManager.getEntitiesWith(RailVehicleComponent.class, RigidBodyComponent.class)) { + for (EntityRef railVehicle : entityManager.getEntitiesWith(RailVehicleComponent.class, + RigidBodyComponent.class)) { updateCart(railVehicle, delta); } } @@ -105,7 +106,8 @@ private void updateCart(EntityRef railVehicle, float delta) { } //checks to see if the cart hits a rail segment - HitResult hit = physics.rayTrace(JomlUtil.from(location.getWorldPosition()), new Vector3f(0, -1, 0), 1.2f, StandardCollisionGroup.DEFAULT, StandardCollisionGroup.WORLD); + HitResult hit = physics.rayTrace(location.getWorldPosition(new Vector3f()), new Vector3f(0, -1, 0), 1.2f, + StandardCollisionGroup.DEFAULT, StandardCollisionGroup.WORLD); if (hit == null || hit.getBlockPosition() == null) { return; } @@ -122,14 +124,16 @@ private void updateCart(EntityRef railVehicle, float delta) { Vector3f position = segmentSystem.segmentPosition(ref); Quaternionf rotation = segmentSystem.segmentRotation(ref); - float segmentPosition = segment.nearestSegmentPosition(JomlUtil.from(location.getWorldPosition()), position, rotation); + float segmentPosition = segment.nearestSegmentPosition(location.getWorldPosition(new Vector3f()), + position, rotation); segmentVehicleComponent.segmentMeta = new SegmentMeta(segmentPosition, ref, prefab); railVehicle.addComponent(segmentVehicleComponent); segmentVehicleComponent.heading = pathFollowerSystem.vehicleTangent(railVehicle); rigidBodyComponent.collidesWith.remove(StandardCollisionGroup.WORLD); - railVehicleComponent.velocity = Util.project(segmentVehicleComponent.heading, rigidBodyComponent.velocity, new Vector3f()); + railVehicleComponent.velocity = Util.project(segmentVehicleComponent.heading, + rigidBodyComponent.velocity, new Vector3f()); if (!railVehicleComponent.velocity.isFinite()) { railVehicleComponent.velocity.set(.001f); } @@ -147,23 +151,25 @@ private void updateCart(EntityRef railVehicle, float delta) { Vector3f normal = pathFollowerSystem.vehicleNormal(railVehicle); Vector3f tangent = pathFollowerSystem.vehicleTangent(railVehicle); - Vector3f gravity = new Vector3f(0, -1, 0).mul(Constants.GRAVITY).mul(delta); railVehicleComponent.velocity.add(Util.project(gravity, tangent, new Vector3f())); - //apply some friction based off the gravity vector projected on the normal multiplied against a friction coff + //apply some friction based off the gravity vector projected on the normal multiplied against a + // friction coff RailComponent rail = segmentVehicleComponent.segmentMeta.association.getComponent(RailComponent.class); Vector3f friction = Util.project(normal, gravity, new Vector3f()).mul(-1).mul(rail.frictionCoefficient); float mag = railVehicleComponent.velocity.length() - friction.length(); - //make sure the magnitude is not less then zero when the friction value is subtracted off of the velocity + //make sure the magnitude is not less then zero when the friction value is subtracted off of the + // velocity if (mag < 0) { mag = 0; } //apply the new velocity to the rail component - railVehicleComponent.velocity = Util.project(railVehicleComponent.velocity, segmentVehicleComponent.heading, new Vector3f()).normalize().mul(mag); + railVehicleComponent.velocity = Util.project(railVehicleComponent.velocity, + segmentVehicleComponent.heading, new Vector3f()).normalize().mul(mag); //make sure the value is not nan or infinite //occurs when the cart hits a perpendicular segment. @@ -172,8 +178,9 @@ private void updateCart(EntityRef railVehicle, float delta) { railVehicleComponent.velocity.set(0); } - if (pathFollowerSystem.move(railVehicle, Math.signum(segmentVehicleComponent.heading.dot(railVehicleComponent.velocity)) * mag * delta, segmentMapping)) { - + if (pathFollowerSystem.move(railVehicle, + Math.signum(segmentVehicleComponent.heading.dot(railVehicleComponent.velocity)) * mag * delta + , segmentMapping)) { //calculate the cart rotation Vector3f horzY = new Vector3f(segmentVehicleComponent.heading); horzY.y = 0; @@ -183,8 +190,8 @@ private void updateCart(EntityRef railVehicle, float delta) { Quaternionf verticalRotation = new Quaternionf().rotateTo(horzY, segmentVehicleComponent.heading); verticalRotation.mul(horizontalRotation); - location.setLocalRotation(JomlUtil.from(verticalRotation)); - location.setWorldPosition(JomlUtil.from(position)); + location.setLocalRotation(verticalRotation); + location.setWorldPosition(position); rigidBodyComponent.kinematic = true; } else { detachFromRail(railVehicle);