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

Example 1 with Matrix3d

use of org.joml.Matrix3d 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 2 with Matrix3d

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

the class TileEntityCaptainsChair method processCalculationsForControlMessageAndApplyCalculations.

private void processCalculationsForControlMessageAndApplyCalculations(PhysicsObject controlledShip, PilotControlsMessage message, IBlockState state) {
    BlockPos chairPosition = getPos();
    if (controlledShip.isShipAligningToGrid()) {
        return;
    }
    double pilotPitch = 0D;
    double pilotYaw = ((BlockCaptainsChair) state.getBlock()).getChairYaw(state, chairPosition);
    double pilotRoll = 0D;
    Matrix3d pilotRotationMatrix = new Matrix3d();
    pilotRotationMatrix.rotateXYZ(Math.toRadians(pilotPitch), Math.toRadians(pilotYaw), Math.toRadians(pilotRoll));
    Vector3d playerDirection = new Vector3d(1, 0, 0);
    pilotRotationMatrix.transform(playerDirection);
    Vector3d upDirection = new Vector3d(0, 1, 0);
    Vector3d downDirection = new Vector3d(0, -1, 0);
    Vector3d idealAngularDirection = new Vector3d();
    Vector3d idealLinearVelocity = new Vector3d();
    Vector3d shipUp = new Vector3d(0, 1, 0);
    Vector3d shipUpPosIdeal = new Vector3d(0, 1, 0);
    if (message.airshipForward_KeyDown) {
        idealLinearVelocity.add(playerDirection);
    }
    if (message.airshipBackward_KeyDown) {
        idealLinearVelocity.sub(playerDirection);
    }
    controlledShip.getShipTransformationManager().getCurrentTickTransform().transformDirection(idealLinearVelocity, TransformType.SUBSPACE_TO_GLOBAL);
    controlledShip.getShipTransformationManager().getCurrentTickTransform().transformDirection(shipUp, TransformType.SUBSPACE_TO_GLOBAL);
    if (message.airshipUp_KeyDown) {
        idealLinearVelocity.add(upDirection.mul(.5, new Vector3d()));
    }
    if (message.airshipDown_KeyDown) {
        idealLinearVelocity.add(downDirection.mul(.5, new Vector3d()));
    }
    double sidePitch = 0;
    if (message.airshipRight_KeyDown) {
        idealAngularDirection.sub(shipUp);
        sidePitch -= 10;
    }
    if (message.airshipLeft_KeyDown) {
        idealAngularDirection.add(shipUp);
        sidePitch += 10;
    }
    Vector3d sidesRotationAxis = new Vector3d(playerDirection);
    controlledShip.getShipTransformationManager().getCurrentTickTransform().transformDirection(sidesRotationAxis, TransformType.SUBSPACE_TO_GLOBAL);
    AxisAngle4d rotationSidesTransform = new AxisAngle4d(Math.toRadians(sidePitch), sidesRotationAxis.x, sidesRotationAxis.y, sidesRotationAxis.z);
    rotationSidesTransform.transform(shipUpPosIdeal);
    idealAngularDirection.mul(2);
    // The vector that points in the direction of the normal of the plane that
    // contains shipUp and shipUpPos. This is our axis of rotation.
    Vector3d shipUpRotationVector = shipUp.cross(shipUpPosIdeal, new Vector3d());
    // This isnt quite right, but it handles the cases quite well.
    double shipUpTheta = shipUp.angle(shipUpPosIdeal) + Math.PI;
    shipUpRotationVector.mul(shipUpTheta);
    idealAngularDirection.add(shipUpRotationVector);
    idealLinearVelocity.mul(20);
    // Move the ship faster if the player holds the sprint key.
    if (message.airshipSprinting) {
        idealLinearVelocity.mul(2);
    }
    double lerpFactor = .2;
    Vector3d linearMomentumDif = controlledShip.getPhysicsCalculations().getLinearVelocity().sub(idealLinearVelocity, new Vector3d());
    Vector3d angularVelocityDif = controlledShip.getPhysicsCalculations().getAngularVelocity().sub(idealAngularDirection, new Vector3d());
    linearMomentumDif.mul(lerpFactor);
    angularVelocityDif.mul(lerpFactor);
    controlledShip.getPhysicsCalculations().getLinearVelocity().sub(linearMomentumDif);
    controlledShip.getPhysicsCalculations().getAngularVelocity().sub(angularVelocityDif);
}

Example 3 with Matrix3d

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

the class PhysicsCalculations method calculateFramedMOITensor.

/**
 * Generates the rotated moment of inertia tensor with the body; uses the following formula: I'
 * = R * I * R-transpose; where I' is the rotated inertia, I is un-rotated interim, and R is the
 * rotation matrix.
 * Reference: https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_matrix_in_different_reference_frames
 */
private void calculateFramedMOITensor() {
    // physCenterOfMass = new Vector(parent.getCenterCoord());
    physTickMass = parent.getInertiaData().getGameTickMass();
    // Copy the rotation matrix, ignore the translation and scaling parts.
    Matrix3dc rotationMatrix = getParent().getShipTransformationManager().getCurrentPhysicsTransform().createRotationMatrix(TransformType.SUBSPACE_TO_GLOBAL);
    Matrix3dc inertiaBodyFrame = parent.getInertiaData().getGameMoITensor();
    Matrix3d rotationMatrixTranspose = rotationMatrix.transpose(new Matrix3d());
    Matrix3d finalInertia = new Matrix3d(rotationMatrix);
    finalInertia.mul(inertiaBodyFrame);
    finalInertia.mul(rotationMatrixTranspose);
    physMOITensor = finalInertia;
    physInvMOITensor = physMOITensor.invert(new Matrix3d());
}