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

Example 36 with Vector3d

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

the class EntityCollisionInjector method getCollidingPolygonsAndDoBlockCols.

/*
     * This method generates an arrayList of Polygons that the player is colliding
     * with
     */
public static ArrayList<Polygon> getCollidingPolygonsAndDoBlockCols(Entity entity, Vec3d velocity) {
    ArrayList<Polygon> collisions = new ArrayList<Polygon>();
    AxisAlignedBB entityBB = entity.getEntityBoundingBox().offset(velocity.x, velocity.y, velocity.z).expand(1, 1, 1);
    List<PhysicsObject> ships = ((IHasShipManager) entity.getEntityWorld()).getManager().getPhysObjectsInAABB(entityBB);
    // and the Player
    for (PhysicsObject wrapper : ships) {
        try {
            Polygon playerInLocal = new Polygon(entityBB, wrapper.getShipTransformationManager().getCurrentTickTransform(), TransformType.GLOBAL_TO_SUBSPACE);
            AxisAlignedBB bb = playerInLocal.getEnclosedAABB();
            if ((bb.maxX - bb.minX) * (bb.maxZ - bb.minZ) > 9898989) {
                // This is too big, something went wrong here
                break;
            }
            List<AxisAlignedBB> collidingBBs = entity.world.getCollisionBoxes(entity, bb);
            // TODO: Fix the performance of this!
            if (entity.world.isRemote || entity instanceof EntityPlayer) {
                VSMath.mergeAABBList(collidingBBs);
            }
            for (AxisAlignedBB inLocal : collidingBBs) {
                ShipPolygon poly = new ShipPolygon(inLocal, wrapper.getShipTransformationManager().getCurrentTickTransform(), TransformType.SUBSPACE_TO_GLOBAL, wrapper.getShipTransformationManager().normals, wrapper);
                collisions.add(poly);
            }
        } catch (Exception e) {
            e.printStackTrace();
        }
    }
    for (PhysicsObject wrapper : ships) {
        double posX = entity.posX;
        double posY = entity.posY;
        double posZ = entity.posZ;
        Vector3d entityPos = new Vector3d(posX, posY, posZ);
        wrapper.getShipTransformationManager().getCurrentTickTransform().transformPosition(entityPos, TransformType.GLOBAL_TO_SUBSPACE);
        setEntityPositionAndUpdateBB(entity, entityPos.x, entityPos.y, entityPos.z);
        int entityChunkX = MathHelper.floor(entity.posX / 16.0D);
        int entityChunkZ = MathHelper.floor(entity.posZ / 16.0D);
        if (wrapper.getChunkClaim().containsChunk(entityChunkX, entityChunkZ)) {
            Chunk chunkIn = wrapper.getChunkAt(entityChunkX, entityChunkZ);
            int chunkYIndex = MathHelper.floor(entity.posY / 16.0D);
            if (chunkYIndex < 0) {
                chunkYIndex = 0;
            }
            if (chunkYIndex >= chunkIn.entityLists.length) {
                chunkYIndex = chunkIn.entityLists.length - 1;
            }
            chunkIn.entityLists[chunkYIndex].add(entity);
            entity.doBlockCollisions();
            chunkIn.entityLists[chunkYIndex].remove(entity);
        }
        setEntityPositionAndUpdateBB(entity, posX, posY, posZ);
    }
    return collisions;
}

Example 37 with Vector3d

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

the class EntityDraggable method applyAddedVelocity.

/**
 * Moves entity forward by addedVelocity, making sure not to clip through blocks
 * @param addedVelocity The velocity added to this entity by the ship they've touched
 * @param entity The entity to be moved
 * @return The vector that the entity actually moved, after detecting collisions with blocks in the world.
 */
public static Vector3dc applyAddedVelocity(final Vector3dc addedVelocity, final Entity entity) {
    double x = addedVelocity.x();
    double y = addedVelocity.y();
    double z = addedVelocity.z();
    if (entity.isInWeb) {
        entity.isInWeb = false;
        x *= 0.25D;
        y *= 0.05000000074505806D;
        z *= 0.25D;
        entity.motionX = 0.0D;
        entity.motionY = 0.0D;
        entity.motionZ = 0.0D;
    }
    double d2 = x;
    double d3 = y;
    double d4 = z;
    AxisAlignedBB potentialCrashBB = entity.getEntityBoundingBox().offset(x, y, z);
    // TODO: This is a band aid not a solution
    if (potentialCrashBB.getAverageEdgeLength() > 999999) {
        // The player went too fast, something is wrong.
        System.err.println("Entity with ID " + entity.getEntityId() + " went way too fast! Reseting its position.");
        return new Vector3d();
    }
    List<AxisAlignedBB> list1 = entity.world.getCollisionBoxes(entity, potentialCrashBB);
    AxisAlignedBB axisalignedbb = entity.getEntityBoundingBox();
    if (y != 0.0D) {
        int k = 0;
        for (int l = list1.size(); k < l; ++k) {
            y = list1.get(k).calculateYOffset(entity.getEntityBoundingBox(), y);
        }
        entity.setEntityBoundingBox(entity.getEntityBoundingBox().offset(0.0D, y, 0.0D));
    }
    if (x != 0.0D) {
        int j5 = 0;
        for (int l5 = list1.size(); j5 < l5; ++j5) {
            x = list1.get(j5).calculateXOffset(entity.getEntityBoundingBox(), x);
        }
        if (x != 0.0D) {
            entity.setEntityBoundingBox(entity.getEntityBoundingBox().offset(x, 0.0D, 0.0D));
        }
    }
    if (z != 0.0D) {
        int k5 = 0;
        for (int i6 = list1.size(); k5 < i6; ++k5) {
            z = list1.get(k5).calculateZOffset(entity.getEntityBoundingBox(), z);
        }
        if (z != 0.0D) {
            entity.setEntityBoundingBox(entity.getEntityBoundingBox().offset(0.0D, 0.0D, z));
        }
    }
    boolean flag = entity.onGround || d3 != y && d3 < 0.0D;
    if (entity.stepHeight > 0.0F && flag && (d2 != x || d4 != z)) {
        double d14 = x;
        double d6 = y;
        double d7 = z;
        AxisAlignedBB axisalignedbb1 = entity.getEntityBoundingBox();
        entity.setEntityBoundingBox(axisalignedbb);
        y = entity.stepHeight;
        List<AxisAlignedBB> list = entity.world.getCollisionBoxes(entity, entity.getEntityBoundingBox().offset(d2, y, d4));
        AxisAlignedBB axisalignedbb2 = entity.getEntityBoundingBox();
        AxisAlignedBB axisalignedbb3 = axisalignedbb2.offset(d2, 0.0D, d4);
        double d8 = y;
        int j1 = 0;
        for (int k1 = list.size(); j1 < k1; ++j1) {
            d8 = list.get(j1).calculateYOffset(axisalignedbb3, d8);
        }
        axisalignedbb2 = axisalignedbb2.offset(0.0D, d8, 0.0D);
        double d18 = d2;
        int l1 = 0;
        for (int i2 = list.size(); l1 < i2; ++l1) {
            d18 = list.get(l1).calculateXOffset(axisalignedbb2, d18);
        }
        axisalignedbb2 = axisalignedbb2.offset(d18, 0.0D, 0.0D);
        double d19 = d4;
        int j2 = 0;
        for (int k2 = list.size(); j2 < k2; ++j2) {
            d19 = list.get(j2).calculateZOffset(axisalignedbb2, d19);
        }
        axisalignedbb2 = axisalignedbb2.offset(0.0D, 0.0D, d19);
        AxisAlignedBB axisalignedbb4 = entity.getEntityBoundingBox();
        double d20 = y;
        int l2 = 0;
        for (int i3 = list.size(); l2 < i3; ++l2) {
            d20 = list.get(l2).calculateYOffset(axisalignedbb4, d20);
        }
        axisalignedbb4 = axisalignedbb4.offset(0.0D, d20, 0.0D);
        double d21 = d2;
        int j3 = 0;
        for (int k3 = list.size(); j3 < k3; ++j3) {
            d21 = list.get(j3).calculateXOffset(axisalignedbb4, d21);
        }
        axisalignedbb4 = axisalignedbb4.offset(d21, 0.0D, 0.0D);
        double d22 = d4;
        int l3 = 0;
        for (int i4 = list.size(); l3 < i4; ++l3) {
            d22 = list.get(l3).calculateZOffset(axisalignedbb4, d22);
        }
        axisalignedbb4 = axisalignedbb4.offset(0.0D, 0.0D, d22);
        double d23 = d18 * d18 + d19 * d19;
        double d9 = d21 * d21 + d22 * d22;
        if (d23 > d9) {
            x = d18;
            z = d19;
            y = -d8;
            entity.setEntityBoundingBox(axisalignedbb2);
        } else {
            x = d21;
            z = d22;
            y = -d20;
            entity.setEntityBoundingBox(axisalignedbb4);
        }
        int j4 = 0;
        for (int k4 = list.size(); j4 < k4; ++j4) {
            y = list.get(j4).calculateYOffset(entity.getEntityBoundingBox(), y);
        }
        entity.setEntityBoundingBox(entity.getEntityBoundingBox().offset(0.0D, y, 0.0D));
        if (d14 * d14 + d7 * d7 >= x * x + z * z) {
            x = d14;
            y = d6;
            z = d7;
            entity.setEntityBoundingBox(axisalignedbb1);
        }
    }
    entity.resetPositionToBB();
    return new Vector3d(x, y, z);
}

Example 38 with Vector3d

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

the class BasicCenterOfMassProvider method addMassAt.

/**
 * Updates the center of mass and rotation inertia tensor matrix of the ShipInertiaData, using the rigid body
 * inertia tensor equations. Reference http://www.kwon3d.com/theory/moi/triten.html eqs. 13 & 14.
 */
private void addMassAt(ShipInertiaData inertiaData, double x, double y, double z, double addedMass) {
    double[] gameMoITensor = new double[9];
    Matrix3d transposed = inertiaData.getGameMoITensor().transpose(new Matrix3d());
    transposed.get(gameMoITensor);
    double gameTickMass = inertiaData.getGameTickMass();
    Vector3d prevCenterOfMass = new Vector3d(inertiaData.getGameTickCenterOfMass());
    if (gameTickMass > .0001D) {
        Vector3d newCenterOfMass = inertiaData.getGameTickCenterOfMass().mul(gameTickMass, new Vector3d());
        newCenterOfMass.add(x * addedMass, y * addedMass, z * addedMass);
        newCenterOfMass.mul(1.0 / (gameTickMass + addedMass));
        inertiaData.setGameTickCenterOfMass(newCenterOfMass);
    } else {
        inertiaData.setGameTickCenterOfMass(new Vector3d(x, y, z));
        inertiaData.setGameMoITensor(new Matrix3d().zero());
    }
    // This code is pretty awful in hindsight, but it gets the job done.
    double cmShiftX = prevCenterOfMass.x - inertiaData.getGameTickCenterOfMass().x();
    double cmShiftY = prevCenterOfMass.y - inertiaData.getGameTickCenterOfMass().y();
    double cmShiftZ = prevCenterOfMass.z - inertiaData.getGameTickCenterOfMass().z();
    double rx = x - inertiaData.getGameTickCenterOfMass().x();
    double ry = y - inertiaData.getGameTickCenterOfMass().y();
    double rz = z - inertiaData.getGameTickCenterOfMass().z();
    gameMoITensor[0] = gameMoITensor[0] + (cmShiftY * cmShiftY + cmShiftZ * cmShiftZ) * gameTickMass + (ry * ry + rz * rz) * addedMass;
    gameMoITensor[1] = gameMoITensor[1] - cmShiftX * cmShiftY * gameTickMass - rx * ry * addedMass;
    gameMoITensor[2] = gameMoITensor[2] - cmShiftX * cmShiftZ * gameTickMass - rx * rz * addedMass;
    gameMoITensor[3] = gameMoITensor[1];
    gameMoITensor[4] = gameMoITensor[4] + (cmShiftX * cmShiftX + cmShiftZ * cmShiftZ) * gameTickMass + (rx * rx + rz * rz) * addedMass;
    gameMoITensor[5] = gameMoITensor[5] - cmShiftY * cmShiftZ * gameTickMass - ry * rz * addedMass;
    gameMoITensor[6] = gameMoITensor[2];
    gameMoITensor[7] = gameMoITensor[5];
    gameMoITensor[8] = gameMoITensor[8] + (cmShiftX * cmShiftX + cmShiftY * cmShiftY) * gameTickMass + (rx * rx + ry * ry) * addedMass;
    inertiaData.setGameMoITensor(new Matrix3d().set(gameMoITensor).transpose());
    // crashing the program.
    if (inertiaData.getGameTickMass() + addedMass < .0001) {
        inertiaData.setGameTickMass(0);
    } else {
        inertiaData.setGameTickMass(inertiaData.getGameTickMass() + addedMass);
    }
}

Example 39 with Vector3d

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

the class ShipTransformationManager method createCollisionNormals.

private static Vector3dc[] createCollisionNormals(ShipTransform transform) {
    // We edit a local array instead of normals to avoid data races.
    final Vector3dc[] newNormals = new Vector3dc[15];
    // Used to generate Normals for the Axis Aligned World
    final Vector3dc[] alignedNorms = Polygon.generateAxisAlignedNorms();
    final Vector3dc[] rotatedNorms = generateRotationNormals(transform);
    for (int i = 0; i < 6; i++) {
        Vector3dc currentNorm;
        if (i < 3) {
            currentNorm = alignedNorms[i];
        } else {
            currentNorm = rotatedNorms[i - 3];
        }
        newNormals[i] = currentNorm;
    }
    int cont = 6;
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            Vector3d norm = newNormals[i].cross(newNormals[j + 3], new Vector3d());
            norm.normalize();
            newNormals[cont] = norm;
            cont++;
        }
    }
    for (int i = 0; i < newNormals.length; i++) {
        if (newNormals[i].lengthSquared() < .01) {
            newNormals[i] = new Vector3d(0.0D, 1.0D, 0.0D);
        }
    }
    newNormals[0] = new Vector3d(1, 0, 0);
    newNormals[1] = new Vector3d(0, 1, 0);
    newNormals[2] = new Vector3d(0, 0, 1);
    return newNormals;
}

Example 40 with Vector3d

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

the class ShipPilot method getTorqueInGlobal.

public Vector3dc getTorqueInGlobal(PhysicsCalculations physicsCalculations) {
    final PhysicsObject physicsObject = physicsCalculations.getParent();
    final ShipTransform shipTransform = physicsObject.getShipTransformationManager().getCurrentPhysicsTransform();
    final Vector3dc idealAngularVelocity = shipTransform.transformDirectionNew(new Vector3d(targetAngularVelocity), TransformType.SUBSPACE_TO_GLOBAL);
    final Vector3dc currentAngularVelocity = physicsCalculations.getAngularVelocity();
    final Vector3dc velocityDifference = idealAngularVelocity.sub(currentAngularVelocity, new Vector3d());
    final Vector3d resultingTorque = physicsCalculations.getPhysMOITensor().transform(velocityDifference, new Vector3d());
    resultingTorque.mul(physicsCalculations.getPhysicsTimeDeltaPerPhysTick());
    resultingTorque.mul(ANGULAR_EMA_FILTER_CONSTANT);
    // Only effect y axis
    resultingTorque.x = 0;
    resultingTorque.z = 0;
    // Add a stabilization torque
    final Vector3dc shipUp = shipTransform.transformDirectionNew(new Vector3d(0, 1, 0), TransformType.SUBSPACE_TO_GLOBAL);
    final Vector3dc idealUp = new Vector3d(0, 1, 0);
    final double angleBetween = shipUp.angle(idealUp);
    if (angleBetween > .01) {
        final Vector3dc stabilizationRotationAxisNormalized = shipUp.cross(idealUp, new Vector3d()).normalize();
        final Vector3d stabilizationTorque = physicsCalculations.getPhysMOITensor().transform(stabilizationRotationAxisNormalized.mul(angleBetween, new Vector3d()));
        stabilizationTorque.mul(physicsCalculations.getPhysicsTimeDeltaPerPhysTick());
        stabilizationTorque.mul(STABILIZATION_TORQUE_CONSTANT);
        resultingTorque.add(stabilizationTorque);
    }
    return resultingTorque;
}