Examples with Vector3d org.joml.Vector3d used on opensource projects

Example 31 with Vector3d

use of org.joml.Vector3d in project Valkyrien-Warfare-Revamped by ValkyrienWarfare.

the class WaterForcesTask method call.

/**
 * Computes the force and torque resulting from the water collisions handled by this task.
 */
@Override
public Void call() {
    final BlockPos.MutableBlockPos currentPos = new BlockPos.MutableBlockPos();
    final ShipTransform physicsTransform = parent.getShipTransformationManager().getCurrentPhysicsTransform();
    final PhysicsCalculations physicsEngine = parent.getPhysicsCalculations();
    // Vector objects reused in this method.
    final Vector3d temp0 = new Vector3d();
    final Vector3d temp1 = new Vector3d();
    final Vector3d temp2 = new Vector3d();
    final Vector3d temp3 = new Vector3d();
    final Vector3d temp4 = new Vector3d();
    final Vector3d temp5 = new Vector3d();
    final Vector3d temp6 = new Vector3d();
    final Vector3d temp7 = new Vector3d();
    final Vector3d temp8 = new Vector3d();
    final Vector3d temp9 = new Vector3d();
    for (int index = minHitIndex; index <= maxHitIndex; index++) {
        final int waterHitPosHash = waterHitsToCheck.get(index);
        SpatialDetector.setPosWithRespectTo(waterHitPosHash, colliderCenter, currentPos);
        final Vector3dc waterPosInShipSpace = physicsTransform.transformPositionNew(JOML.convertDouble(currentPos, temp0).add(.5, .5, .5), TransformType.GLOBAL_TO_SUBSPACE);
        final int minX = (int) Math.floor(waterPosInShipSpace.x() - .5);
        final int minY = (int) Math.floor(waterPosInShipSpace.y() - .5);
        final int minZ = (int) Math.floor(waterPosInShipSpace.z() - .5);
        final int maxX = (int) Math.ceil(waterPosInShipSpace.x() + .5);
        final int maxY = (int) Math.ceil(waterPosInShipSpace.y() + .5);
        final int maxZ = (int) Math.ceil(waterPosInShipSpace.z() + .5);
        final Vector3dc waterPosInWorld = JOML.convertDouble(currentPos, temp1).add(.5, .5, .5);
        for (int x = minX; x <= maxX; x++) {
            for (int z = minZ; z <= maxZ; z++) {
                final Chunk chunk = parent.getChunkClaim().containsChunk(x >> 4, z >> 4) ? parent.getChunkAt(x >> 4, z >> 4) : null;
                if (chunk == null)
                    continue;
                for (int y = minY; y <= maxY; y++) {
                    final ExtendedBlockStorage blockStorage = chunk.storageArrays[y >> 4];
                    if (blockStorage != null) {
                        final IBitOctree terrainOctree = ((ITerrainOctreeProvider) blockStorage.data).getSolidOctree();
                        if (terrainOctree.get(x & 15, y & 15, z & 15)) {
                            // Assume both the water block and terrain block are spheres, then compute the volume
                            // that overlaps
                            final Vector3dc shipSolidBlockPosInWorld = physicsTransform.transformPositionNew(temp2.set(x + .5, y + .5, z + .5), TransformType.SUBSPACE_TO_GLOBAL);
                            final double volumeDisplaced = calculateAABBOverlap(waterPosInWorld.x() - shipSolidBlockPosInWorld.x(), waterPosInWorld.y() - shipSolidBlockPosInWorld.y(), waterPosInWorld.z() - shipSolidBlockPosInWorld.z());
                            if (volumeDisplaced <= 0) {
                                // No intersection
                                continue;
                            }
                            // Collision position is average of ship solid block pos and water pos
                            final Vector3dc collisionPosInWorld = shipSolidBlockPosInWorld.add(waterPosInWorld, temp3).mul(.5);
                            final Vector3dc buoyancyForce = temp4.set(0, volumeDisplaced * GRAVITY_ACCELERATION * MASS_OF_CUBIC_METER_OF_WATER, 0);
                            final Vector3dc collisionPosRelativeToShipCenterInWorld = temp5.set(collisionPosInWorld).sub(physicsTransform.getPosX(), physicsTransform.getPosY(), physicsTransform.getPosZ());
                            addForceAtPoint(collisionPosRelativeToShipCenterInWorld, buoyancyForce, temp7);
                            {
                                // Compute water damping force
                                final Vector3dc velocity = physicsEngine.getVelocityAtPoint(collisionPosRelativeToShipCenterInWorld, temp9);
                                if (!isVectorLengthZero(velocity)) {
                                    // TODO: This is WRONG, but it'll do for now
                                    // The distance between the water block and the solid block its pushing upwards
                                    double distance = waterPosInWorld.distance(shipSolidBlockPosInWorld);
                                    final double area = Math.PI * (SPHERE_RADIUS - (distance * .5)) * (SPHERE_RADIUS - (distance * .5));
                                    final double velocitySquared = velocity.lengthSquared();
                                    // Drag formula from https://en.wikipedia.org/wiki/Drag_(physics)
                                    final double forceMagnitude = (.5) * DENSITY_OF_WATER * velocitySquared * DRAG_COEFFICIENT_OF_WATER * area;
                                    final Vector3dc dragForce = temp6.set(velocity).normalize().mul(-forceMagnitude);
                                    addForceAtPoint(collisionPosRelativeToShipCenterInWorld, dragForce, temp8);
                                }
                            }
                        }
                    }
                }
            }
        }
    }
    return null;
}

Example 32 with Vector3d

use of org.joml.Vector3d in project Valkyrien-Warfare-Revamped by ValkyrienWarfare.

the class WorldPhysicsCollider method handleCollision.

private void handleCollision(final Vector3dc collisionPos, final Vector3dc axis, final Vector3dc offsetVector, final double impulseApplied) {
    Vector3d inBody = new Vector3d(collisionPos.x() - parent.getShipTransform().getPosX(), collisionPos.y() - parent.getShipTransform().getPosY(), collisionPos.z() - parent.getShipTransform().getPosZ());
    Vector3d momentumAtPoint = calculator.getVelocityAtPoint(inBody);
    calculateCollisionImpulseForce(inBody, momentumAtPoint, axis, offsetVector, false, false, impulseApplied);
}

Example 33 with Vector3d

use of org.joml.Vector3d in project Valkyrien-Warfare-Revamped by ValkyrienWarfare.

the class WorldPhysicsCollider method updateCollisionCacheSequential.

private void updateCollisionCacheSequential(ChunkCache cache, int chunkX, int chunkZ, int minX, int minY, int minZ, int maxX, int maxY, int maxZ, AxisAlignedBB shipBB, TIntList output) {
    int arrayChunkX = chunkX - cache.chunkX;
    int arrayChunkZ = chunkZ - cache.chunkZ;
    if (!(arrayChunkX < 0 || arrayChunkZ < 0 || arrayChunkX > cache.chunkArray.length - 1 || arrayChunkZ > cache.chunkArray[0].length - 1) && cache.chunkArray[arrayChunkX][arrayChunkZ] != null) {
        Vector3d temp1 = new Vector3d();
        Vector3d temp2 = new Vector3d();
        Vector3d temp3 = new Vector3d();
        Chunk chunk = cache.chunkArray[arrayChunkX][arrayChunkZ];
        for (int storageY = minY >> 4; storageY <= maxY >> 4; storageY++) {
            ExtendedBlockStorage extendedblockstorage = chunk.storageArrays[storageY];
            if (extendedblockstorage != null) {
                int minStorageX = chunkX << 4;
                int minStorageY = storageY << 4;
                int minStorageZ = chunkZ << 4;
                int maxStorageX = minStorageX + 16;
                int maxStorageY = minStorageY + 16;
                int maxStorageZ = minStorageZ + 16;
                final ITerrainOctreeProvider provider = (ITerrainOctreeProvider) extendedblockstorage.data;
                final IBitOctree octree = provider.getSolidOctree();
                for (int x = minStorageX; x < maxStorageX; x++) {
                    for (int y = minStorageY; y < maxStorageY; y++) {
                        for (int z = minStorageZ; z < maxStorageZ; z++) {
                            checkForCollision(x, y, z, extendedblockstorage, octree, temp1, temp2, temp3, shipBB, output);
                        }
                    }
                }
            }
        }
    }
}

Example 34 with Vector3d

use of org.joml.Vector3d in project Valkyrien-Warfare-Revamped by ValkyrienWarfare.

the class WorldPhysicsCollider method calculateCollisionImpulseForce.

// Finally, the end of all this spaghetti code! This step takes all of the math
// generated before, and it directly adds the result to Ship velocities
private void calculateCollisionImpulseForce(Vector3dc inBody, Vector3d velocityAtPointOfCollision, Vector3dc axis, Vector3dc offsetVector, boolean didBlockBreakInShip, boolean didBlockBreakInWorld, double impulseApplied) {
    // Our ideal velocity is to negate the velocity at the point, and the collision offset.
    final double multiplier = 2;
    velocityAtPointOfCollision.add(offsetVector.x() * multiplier, offsetVector.y() * multiplier, offsetVector.z() * multiplier);
    Vector3d firstCross = inBody.cross(axis, new Vector3d());
    calculator.getPhysInvMOITensor().transform(firstCross);
    Vector3d secondCross = firstCross.cross(inBody);
    double impulseMagnitude = -velocityAtPointOfCollision.dot(axis) / (calculator.getInvMass() + secondCross.dot(axis));
    // Below this speed our collision coefficient of restitution is zero.
    final double slopR = .5D;
    double collisionSpeed = Math.abs(velocityAtPointOfCollision.dot(axis));
    if (collisionSpeed > slopR) {
        impulseMagnitude *= (1 + COEFFICIENT_OF_RESTITUTION);
    } else {
    // TODO: Need to reduce this value by some factor
    // impulseMagnitude *= .5D;
    }
    Vector3d collisionImpulseForce = axis.mul(impulseMagnitude, new Vector3d());
    // the direction towards the in body vector.
    if (collisionImpulseForce.dot(offsetVector) < 0 && collisionImpulseForce.dot(inBody) < 0) {
        // collisionImpulseForce.multiply(1.8D);
        double collisionVelocity = velocityAtPointOfCollision.dot(axis);
        addFrictionToNormalForce(velocityAtPointOfCollision, collisionImpulseForce, inBody);
        calculator.getLinearVelocity().add(collisionImpulseForce.mul(calculator.getInvMass(), new Vector3d()));
        Vector3d thirdCross = inBody.cross(collisionImpulseForce, new Vector3d());
        calculator.getPhysInvMOITensor().transform(thirdCross);
        calculator.getAngularVelocity().add(thirdCross, calculator.getAngularVelocity());
    }
}

Example 35 with Vector3d

use of org.joml.Vector3d in project Valkyrien-Warfare-Revamped by ValkyrienWarfare.

the class EntityCollisionInjector method alterEntityMovement.

// Returns false if game should use default collision
@Nullable
public static IntermediateMovementVariableStorage alterEntityMovement(Entity entity, MoverType type, double dx, double dy, double dz) {
    final double origDx = dx;
    final double origDy = dy;
    final double origDz = dz;
    final double origPosX = entity.posX;
    final double origPosY = entity.posY;
    final double origPosZ = entity.posZ;
    boolean isLiving = entity instanceof EntityLivingBase;
    Vec3d velocity = new Vec3d(dx, dy, dz);
    Polygon playerBeforeMove = new Polygon(entity.getEntityBoundingBox());
    List<Polygon> colPolys = getCollidingPolygonsAndDoBlockCols(entity, velocity);
    PhysicsObject worldBelow = null;
    IDraggable draggable = EntityDraggable.getDraggableFromEntity(entity);
    final EntityShipMovementData lastTickEntityShipMovementData = draggable.getEntityShipMovementData();
    Vector3d total = new Vector3d();
    // Used to reset the player position after collision processing, effectively
    // using the player to integrate their velocity
    double posOffestX = 0;
    double posOffestY = 0;
    double posOffestZ = 0;
    // True IFF the player is on a ladder
    boolean isPlayerOnLadder = false;
    // region Ladder movement
    if (entity instanceof EntityLivingBase) {
        final EntityLivingBase base = (EntityLivingBase) entity;
        final List<PhysicsObject> collidingShips = ((IHasShipManager) entity.getEntityWorld()).getManager().getPhysObjectsInAABB(base.getEntityBoundingBox());
        final Iterable<Triple<PhysicsObject, BlockPos, IBlockState>> ladderCollisions = getLadderCollisions(base, collidingShips);
        // For now, just ignore the y component. I may or may not use it later.
        final float forward = ((EntityLivingBase) entity).moveForward;
        final float strafe = ((EntityLivingBase) entity).moveStrafing;
        final double f1 = Math.sin(Math.toRadians(entity.rotationYaw));
        final double f2 = Math.cos(Math.toRadians(entity.rotationYaw));
        final double intendedXVel = strafe * f2 - forward * f1;
        final double intendedYVel = 0;
        final double intendedZVel = forward * f2 + strafe * f1;
        final Vector3dc originalVelocityDirection = new Vector3d(intendedXVel, intendedYVel, intendedZVel).normalize();
        final World world = entity.world;
        final Polygon playerPolygon = new Polygon(base.getEntityBoundingBox());
        for (final Triple<PhysicsObject, BlockPos, IBlockState> ladderCollision : ladderCollisions) {
            final IBlockState ladderState = ladderCollision.getRight();
            EnumFacing ladderFacing = null;
            // For now, we only support a few blocks
            if (ladderState.getPropertyKeys().contains(BlockHorizontal.FACING)) {
                ladderFacing = ladderState.getValue(BlockHorizontal.FACING);
            }
            // We need the EnumFacing of the ladder for the code to work. If we couldn't find it then just give up :/
            if (ladderFacing != null) {
                final Vector3d ladderNormal = JOML.convertDouble(ladderFacing.getDirectionVec());
                final ShipTransform shipTransform = ladderCollision.getLeft().getShipTransform();
                // Grow the ladder BB by a small margin (makes the ladder experience better imo)
                final AxisAlignedBB ladderBB = ladderCollision.getRight().getBoundingBox(world, ladderCollision.getMiddle()).offset(ladderCollision.getMiddle()).grow(.4);
                final Polygon ladderPoly = new Polygon(ladderBB, shipTransform.getSubspaceToGlobal());
                // Determine if the player is actually colliding with the ladder
                final PhysPolygonCollider collider = new PhysPolygonCollider(playerPolygon, ladderPoly, ladderCollision.getLeft().getShipTransformationManager().normals);
                collider.processData();
                shipTransform.transformDirection(ladderNormal, TransformType.SUBSPACE_TO_GLOBAL);
                // Don't use "floor ladders"
                final boolean isLadderFacingDown = ladderNormal.y > .8;
                if (isLadderFacingDown) {
                    continue;
                }
                // If the ladder is facing up, then let the player use them like monkey bars
                final boolean isLadderFacingUp = ladderNormal.y < -.8;
                // Whether or not the player is actually colliding with a ladder, since it is close to one we give the player ladder movement.
                dx = MathHelper.clamp(dx, -.15, .15);
                dz = MathHelper.clamp(dz, -.15, .15);
                base.fallDistance = 0;
                if (!isLadderFacingUp) {
                    // Use ladders like normal
                    if (dy < -.15) {
                        dy = -.15;
                    }
                    final boolean isPlayerGoingTowardsLadder = originalVelocityDirection.dot(ladderNormal) < -.1;
                    final boolean isPlayerSneakingOnLadder = base.isSneaking() && base instanceof EntityPlayer;
                    if (isPlayerSneakingOnLadder && dy < 0) {
                        dy = 0;
                    }
                    if (!collider.seperated && isPlayerGoingTowardsLadder) {
                        dy = .2;
                    }
                } else {
                    // Use ladders like monkey bars
                    dy = .2;
                }
                worldBelow = ladderCollision.getLeft();
                isPlayerOnLadder = true;
                break;
            }
        }
    }
    // endregion
    final Vector3dc velVec = new Vector3d(dx, dy, dz);
    for (Polygon poly : colPolys) {
        if (poly instanceof ShipPolygon) {
            ShipPolygon shipPoly = (ShipPolygon) poly;
            try {
                EntityPolygonCollider fast = new EntityPolygonCollider(playerBeforeMove, shipPoly, shipPoly.normals, velVec.add(total, new Vector3d()));
                if (!fast.arePolygonsSeparated()) {
                    // fastCollisions.add(fast);
                    worldBelow = shipPoly.shipFrom;
                    Vector3d response = fast.getCollisions()[fast.getMinDistanceIndex()].getResponse();
                    // TODO: Add more potential yResponses
                    double stepSquared = entity.stepHeight * entity.stepHeight;
                    // Do not do stair stepping if the player is on a ladder.
                    boolean isStep = isLiving && entity.onGround && !isPlayerOnLadder;
                    if (response.y >= 0 && VSMath.canStandOnNormal(fast.getCollisionAxes()[fast.getMinDistanceIndex()])) {
                        Vector3d slowButStopped = new Vector3d(0, -fast.getCollisions()[fast.getMinDistanceIndex()].getCollisionPenetrationDistance() / fast.getCollisionAxes()[fast.getMinDistanceIndex()].y(), 0);
                        response = slowButStopped;
                    }
                    if (isStep) {
                        EntityLivingBase living = (EntityLivingBase) entity;
                        if (Math.abs(living.moveForward) > .01 || Math.abs(living.moveStrafing) > .01) {
                            for (int i = 3; i < 6; i++) {
                                Vector3d tempResponse = fast.getCollisions()[i].getResponse();
                                if (tempResponse.y > 0 && VSMath.canStandOnNormal(fast.getCollisions()[i].getCollisionNormal()) && tempResponse.lengthSquared() < stepSquared) {
                                    if (tempResponse.lengthSquared() < .1) {
                                        // Too small to be a real step, let it through
                                        response = tempResponse;
                                    } else {
                                        // System.out.println("Try Stepping!");
                                        AxisAlignedBB axisalignedbb = entity.getEntityBoundingBox().offset(tempResponse.x, tempResponse.y, tempResponse.z);
                                        // Don't allow the player to step if the step will put them in another polygon.
                                        boolean collidesWithAnything = false;
                                        {
                                            final AxisAlignedBB newEntityBBShrunk = axisalignedbb.shrink(.15);
                                            final Polygon newEntityBBShrunkPolygon = new Polygon(newEntityBBShrunk);
                                            for (Polygon potentialStepCollision : colPolys) {
                                                if (potentialStepCollision == poly) {
                                                    // Don't run this on ourself
                                                    continue;
                                                }
                                                if (potentialStepCollision.getEnclosedAABB().intersects(newEntityBBShrunk)) {
                                                    // Finer check
                                                    ShipPolygon potentialStepCollisionShipPoly = (ShipPolygon) potentialStepCollision;
                                                    final EntityPolygonCollider checkIfStepCollidesWithBlock = new EntityPolygonCollider(newEntityBBShrunkPolygon, potentialStepCollisionShipPoly, potentialStepCollisionShipPoly.normals, new Vector3d());
                                                    checkIfStepCollidesWithBlock.processData();
                                                    if (!checkIfStepCollidesWithBlock.arePolygonsSeparated()) {
                                                        collidesWithAnything = true;
                                                        break;
                                                    }
                                                }
                                            }
                                        }
                                        if (!collidesWithAnything) {
                                            entity.setEntityBoundingBox(axisalignedbb);
                                            // I think this correct, but it may create more problems than it solves
                                            response.zero();
                                            entity.resetPositionToBB();
                                        }
                                    }
                                }
                            }
                        }
                    }
                    // total.add(response);
                    if (Math.abs(response.x) > .01D) {
                        total.x += response.x;
                    }
                    if (Math.abs(response.y) > .01D) {
                        total.y += response.y;
                    }
                    if (Math.abs(response.z) > .01D) {
                        total.z += response.z;
                    }
                    entity.posX += response.x;
                    entity.posY += response.y;
                    entity.posZ += response.z;
                    posOffestX += response.x;
                    posOffestY += response.y;
                    posOffestZ += response.z;
                    AxisAlignedBB axisalignedbb = entity.getEntityBoundingBox().offset(response.x, response.y, response.z);
                    entity.setEntityBoundingBox(axisalignedbb);
                    entity.resetPositionToBB();
                }
            } catch (Exception e) {
            // Do nothing
            }
        }
    }
    AxisAlignedBB axisalignedbb = entity.getEntityBoundingBox().offset(-posOffestX, -posOffestY, -posOffestZ);
    entity.setEntityBoundingBox(axisalignedbb);
    entity.resetPositionToBB();
    // We are on the ship that we are riding
    if (entity.ridingEntity instanceof EntityMountable) {
        final EntityMountable entityMountable = (EntityMountable) entity.ridingEntity;
        if (entityMountable.getReferencePosOptional().isPresent()) {
            final Optional<PhysicsObject> physicsObjectOptional = ValkyrienUtils.getPhysoManagingBlock(entity.world, entityMountable.getReferencePosOptional().get());
            if (physicsObjectOptional.isPresent()) {
                worldBelow = physicsObjectOptional.get();
            }
        }
    }
    if (worldBelow == null) {
        return null;
    }
    dx += total.x;
    dy += total.y;
    dz += total.z;
    boolean alreadyOnGround = entity.onGround && (dy == origDy) && origDy < 0;
    Vector3d original = new Vector3d(origDx, origDy, origDz);
    Vector3d newMov = new Vector3d(dx - origDx, dy - origDy, dz - origDz);
    entity.collidedHorizontally = original.dot(newMov) < 0;
    entity.collidedVertically = isDifSignificant(dy, origDy);
    entity.onGround = entity.collidedVertically && origDy < 0 || alreadyOnGround;
    entity.collided = entity.collidedHorizontally || entity.collidedVertically;
    // entity.resetPositionToBB();
    double motionYBefore = entity.motionY;
    float oldFallDistance = entity.fallDistance;
    Vector3d dxyz = new Vector3d(dx, dy, dz);
    ;
    Vector3d origDxyz = new Vector3d(origDx, origDy, origDz);
    Vector3d origPosXyz = new Vector3d(origPosX, origPosY, origPosZ);
    return new IntermediateMovementVariableStorage(dxyz, origDxyz, origPosXyz, alreadyOnGround, motionYBefore, oldFallDistance, worldBelow.getShipData());
}