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

Example 41 with Vector3dc

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

the class PhysicsCalculations method integrateAngularVelocity.

/**
 * Implementation is based on https://gafferongames.com/post/physics_in_3d/
 */
private void integrateAngularVelocity() {
    // The body angular velocity vector, in World coordinates
    if (angularVelocity.lengthSquared() < .001) {
        // Angular velocity is zero, so the rotation hasn't changed.
        return;
    }
    // If a ship is rotating too fast, then clamp its max rotational speed
    if (angularVelocity.lengthSquared() > VSConfig.shipMaxAngularSpeed * VSConfig.shipMaxAngularSpeed) {
        angularVelocity.normalize().mul(VSConfig.shipMaxAngularSpeed);
    }
    Vector3dc angularVelInBody = new Vector3d(angularVelocity);
    AxisAngle4d axisAngle4d = new AxisAngle4d(angularVelInBody.length() * getPhysicsTimeDeltaPerPhysTick(), angularVelInBody.x(), angularVelInBody.y(), angularVelInBody.z());
    axisAngle4d.normalize();
    // Take the product of the current rotation with the change in rotation that results from
    // the angular velocity. Then change our pitch/yaw/roll based on the result.
    Quaterniondc rotationQuat = new Quaterniond(axisAngle4d);
    physRotation = physRotation.premul(rotationQuat, new Quaterniond()).normalize();
}

Example 42 with Vector3dc

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

the class PhysicsCalculations method calculateForces.

private void calculateForces() {
    applyAirDrag();
    applyGravity();
    Vector3d blockForce = new Vector3d();
    Vector3d inBodyWO = new Vector3d();
    Vector3d crossVector = new Vector3d();
    World worldObj = getParent().getWorld();
    if (VSConfig.doPhysicsBlocks) {
        // We want to loop through all the physics nodes in a sorted order. Priority Queue handles that.
        Queue<IPhysicsBlockController> nodesPriorityQueue = new PriorityQueue<>(parent.getPhysicsControllersInShip());
        while (nodesPriorityQueue.size() > 0) {
            IPhysicsBlockController controller = nodesPriorityQueue.poll();
            controller.onPhysicsTick(parent, this, this.getPhysicsTimeDeltaPerPhysTick());
        }
        SortedMap<IBlockTorqueProvider, List<BlockPos>> torqueProviders = new TreeMap<>();
        BlockPos.MutableBlockPos mutablePos = new BlockPos.MutableBlockPos();
        // Note that iterating over "activeForcePositions" is not thread safe, so this could lead to problems.
        // However we're going to completely replace this code soon anyways, so its not a big deal.
        parent.getShipData().activeForcePositions.forEachUnsafe((x, y, z) -> {
            mutablePos.setPos(x, y, z);
            IBlockState state = getParent().getChunkAt(mutablePos.getX() >> 4, mutablePos.getZ() >> 4).getBlockState(mutablePos);
            Block blockAt = state.getBlock();
            if (blockAt instanceof IBlockForceProvider) {
                try {
                    BlockPhysicsDetails.getForceFromState(state, mutablePos, worldObj, getPhysicsTimeDeltaPerPhysTick(), getParent(), blockForce);
                    Vector3dc otherPosition = ((IBlockForceProvider) blockAt).getCustomBlockForcePosition(worldObj, mutablePos, state, getParent(), getPhysicsTimeDeltaPerPhysTick());
                    if (otherPosition != null) {
                        inBodyWO.set(otherPosition);
                        inBodyWO.sub(physCenterOfMass);
                        getParent().getShipTransformationManager().getCurrentPhysicsTransform().transformDirection(inBodyWO, TransformType.SUBSPACE_TO_GLOBAL);
                    } else {
                        inBodyWO.set(mutablePos.getX() + .5, mutablePos.getY() + .5, mutablePos.getZ() + .5);
                        inBodyWO.sub(physCenterOfMass);
                        getParent().getShipTransformationManager().getCurrentPhysicsTransform().transformDirection(inBodyWO, TransformType.SUBSPACE_TO_GLOBAL);
                    }
                    addForceAtPoint(inBodyWO, blockForce, crossVector);
                } catch (Exception e) {
                    e.printStackTrace();
                }
            }
            if (blockAt instanceof IBlockTorqueProvider) {
                // Add it to the torque sorted map; we do this so the torque dampeners can run
                // after the gyroscope stabilizers.
                IBlockTorqueProvider torqueProviderBlock = (IBlockTorqueProvider) blockAt;
                if (!torqueProviders.containsKey(torqueProviderBlock)) {
                    torqueProviders.put(torqueProviderBlock, new LinkedList<>());
                }
                torqueProviders.get(torqueProviderBlock).add(new BlockPos(x, y, z));
            }
        });
        // Now add the torque from the torque providers, in a sorted order!
        for (IBlockTorqueProvider torqueProviderBlock : torqueProviders.keySet()) {
            List<BlockPos> blockPositions = torqueProviders.get(torqueProviderBlock);
            for (BlockPos pos : blockPositions) {
                this.convertTorqueToVelocity();
                Vector3dc torqueVector = torqueProviderBlock.getTorqueInGlobal(this, pos);
                if (torqueVector != null) {
                    torque.add(torqueVector);
                }
            }
        }
    }
    // Add pilot input
    final ShipPilot parentPilot = parent.getShipPilot();
    if (parentPilot != null) {
        final Vector3dc pilotForce = parentPilot.getBlockForceInShipSpace(parent, physTickTimeDelta);
        final Vector3dc pilotTorque = parentPilot.getTorqueInGlobal(this);
        this.linearVelocity.fma(getInvMass(), pilotForce);
        this.torque.add(pilotTorque);
    }
    convertTorqueToVelocity();
}

Example 43 with Vector3dc

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

the class PhysCollisionObject method generateCollision.

public void generateCollision() {
    playerMinMax = VSMath.getMinMaxOfArray(movable.getProjectionOnVector(collision_normal));
    blockMinMax = VSMath.getMinMaxOfArray(fixed.getProjectionOnVector(collision_normal));
    movMaxFixMin = playerMinMax[0] - blockMinMax[1];
    movMinFixMax = playerMinMax[1] - blockMinMax[0];
    if (movMaxFixMin > 0 || movMinFixMax < 0) {
        seperated = true;
        penetrationDistance = 0.0D;
        return;
    }
    // Set the penetration to be the smaller distance
    if (Math.abs(movMaxFixMin) > Math.abs(movMinFixMax)) {
        penetrationDistance = movMinFixMax;
        for (Vector3dc v : movable.getVertices()) {
            if (v.dot(collision_normal) == playerMinMax[1]) {
                firstContactPoint = v;
            }
        }
    } else {
        penetrationDistance = movMaxFixMin;
        for (Vector3dc v : movable.getVertices()) {
            if (v.dot(collision_normal) == playerMinMax[0]) {
                firstContactPoint = v;
            }
        }
    }
    seperated = false;
}

Example 44 with Vector3dc

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

the class Polygon method getCenter.

public Vector3d getCenter() {
    Vector3d center = new Vector3d();
    for (Vector3dc v : vertices) {
        center.add(v);
    }
    center.mul(1.0 / vertices.length);
    return center;
}

Example 45 with Vector3dc

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

the class ShipCollisionTask method checkPosition.

public void checkPosition(int x, int y, int z, int positionHash) {
    if (!toTask.getParent().getChunkClaim().containsChunk(x >> 4, z >> 4)) {
        return;
    }
    final Chunk chunkIn = toTask.getParent().getChunkAt(x >> 4, z >> 4);
    y = Math.max(0, Math.min(y, 255));
    ExtendedBlockStorage storage = chunkIn.storageArrays[y >> 4];
    if (storage != null) {
        ITerrainOctreeProvider provider = (ITerrainOctreeProvider) storage.data;
        IBitOctree octree = provider.getSolidOctree();
        if (octree.get(x & 15, y & 15, z & 15)) {
            IBlockState inLocalState = chunkIn.getBlockState(x, y, z);
            inLocalPos.setPos(x, y, z);
            final ShipTransform shipTransform = toTask.getParent().getShipTransformationManager().getCurrentPhysicsTransform();
            final Vector3dc shipBlockInGlobal = shipTransform.transformPositionNew(temp0.set(inLocalPos.getX() + .5, inLocalPos.getY() + .5, inLocalPos.getZ() + .5), TransformType.SUBSPACE_TO_GLOBAL);
            final double distanceSq = shipBlockInGlobal.distanceSquared(mutablePos.getX() + .5, mutablePos.getY() + .5, mutablePos.getZ() + .5);
            // If it is less than sqrt(3) then collision is possible.
            if (distanceSq < 3) {
                CollisionInformationHolder holder = new CollisionInformationHolder(mutablePos.getX(), mutablePos.getY(), mutablePos.getZ(), inLocalPos.getX(), inLocalPos.getY(), inLocalPos.getZ(), inWorldState, inLocalState);
                collisionInformationGenerated.add(holder);
            }
        }
    }
}