Examples with Matrix3f javax.vecmath.Matrix3f used on opensource projects

Example 1 with Matrix3f

use of javax.vecmath.Matrix3f in project MinecraftForge by MinecraftForge.

the class ForgeHooksClient method applyUVLock.

public static BlockFaceUV applyUVLock(BlockFaceUV blockFaceUV, EnumFacing originalSide, ITransformation rotation) {
    TRSRTransformation global = new TRSRTransformation(rotation.getMatrix());
    Matrix4f uv = global.getUVLockTransform(originalSide).getMatrix();
    Vector4f vec = new Vector4f(0, 0, 0, 1);
    vec.x = blockFaceUV.getVertexU(blockFaceUV.getVertexRotatedRev(0)) / 16;
    vec.y = blockFaceUV.getVertexV(blockFaceUV.getVertexRotatedRev(0)) / 16;
    uv.transform(vec);
    // / vec.w;
    float uMin = 16 * vec.x;
    // / vec.w;
    float vMin = 16 * vec.y;
    vec.x = blockFaceUV.getVertexU(blockFaceUV.getVertexRotatedRev(2)) / 16;
    vec.y = blockFaceUV.getVertexV(blockFaceUV.getVertexRotatedRev(2)) / 16;
    vec.z = 0;
    vec.w = 1;
    uv.transform(vec);
    // / vec.w;
    float uMax = 16 * vec.x;
    // / vec.w;
    float vMax = 16 * vec.y;
    if (uMin > uMax) {
        float t = uMin;
        uMin = uMax;
        uMax = t;
    }
    if (vMin > vMax) {
        float t = vMin;
        vMin = vMax;
        vMax = t;
    }
    float a = (float) Math.toRadians(blockFaceUV.rotation);
    Vector3f rv = new Vector3f(MathHelper.cos(a), MathHelper.sin(a), 0);
    Matrix3f rot = new Matrix3f();
    uv.getRotationScale(rot);
    rot.transform(rv);
    int angle = MathHelper.normalizeAngle(-(int) Math.round(Math.toDegrees(Math.atan2(rv.y, rv.x)) / 90) * 90, 360);
    return new BlockFaceUV(new float[] { uMin, vMin, uMax, vMax }, angle);
}

Example 2 with Matrix3f

use of javax.vecmath.Matrix3f in project bdx by GoranM.

the class CollisionWorld method objectQuerySingle.

/**
	 * objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
	 */
public static void objectQuerySingle(ConvexShape castShape, Transform convexFromTrans, Transform convexToTrans, CollisionObject collisionObject, CollisionShape collisionShape, Transform colObjWorldTransform, ConvexResultCallback resultCallback, float allowedPenetration) {
    Stack stack = Stack.enter();
    if (collisionShape.isConvex()) {
        CastResult castResult = new CastResult();
        castResult.allowedPenetration = allowedPenetration;
        // ??
        castResult.fraction = 1f;
        ConvexShape convexShape = (ConvexShape) collisionShape;
        VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
        GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = new GjkEpaPenetrationDepthSolver();
        // JAVA TODO: should be convexCaster1
        //ContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
        GjkConvexCast convexCaster2 = new GjkConvexCast(castShape, convexShape, simplexSolver);
        //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
        ConvexCast castPtr = convexCaster2;
        if (castPtr.calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) {
            // add hit
            if (castResult.normal.lengthSquared() > 0.0001f) {
                if (castResult.fraction < resultCallback.closestHitFraction) {
                    castResult.normal.normalize();
                    LocalConvexResult localConvexResult = new LocalConvexResult(collisionObject, null, castResult.normal, castResult.hitPoint, castResult.fraction);
                    boolean normalInWorldSpace = true;
                    resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
                }
            }
        }
    } else {
        if (collisionShape.isConcave()) {
            if (collisionShape.getShapeType() == BroadphaseNativeType.TRIANGLE_MESH_SHAPE_PROXYTYPE) {
                BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape) collisionShape;
                Transform worldTocollisionObject = stack.allocTransform();
                worldTocollisionObject.inverse(colObjWorldTransform);
                Vector3f convexFromLocal = stack.allocVector3f();
                convexFromLocal.set(convexFromTrans.origin);
                worldTocollisionObject.transform(convexFromLocal);
                Vector3f convexToLocal = stack.allocVector3f();
                convexToLocal.set(convexToTrans.origin);
                worldTocollisionObject.transform(convexToLocal);
                // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
                Transform rotationXform = stack.allocTransform();
                Matrix3f tmpMat = stack.allocMatrix3f();
                tmpMat.mul(worldTocollisionObject.basis, convexToTrans.basis);
                rotationXform.set(tmpMat);
                BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, convexFromTrans, convexToTrans, resultCallback, collisionObject, triangleMesh, colObjWorldTransform);
                tccb.hitFraction = resultCallback.closestHitFraction;
                tccb.normalInWorldSpace = true;
                Vector3f boxMinLocal = stack.allocVector3f();
                Vector3f boxMaxLocal = stack.allocVector3f();
                castShape.getAabb(rotationXform, boxMinLocal, boxMaxLocal);
                triangleMesh.performConvexcast(tccb, convexFromLocal, convexToLocal, boxMinLocal, boxMaxLocal);
            } else {
                BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape) collisionShape;
                Transform worldTocollisionObject = stack.allocTransform();
                worldTocollisionObject.inverse(colObjWorldTransform);
                Vector3f convexFromLocal = stack.allocVector3f();
                convexFromLocal.set(convexFromTrans.origin);
                worldTocollisionObject.transform(convexFromLocal);
                Vector3f convexToLocal = stack.allocVector3f();
                convexToLocal.set(convexToTrans.origin);
                worldTocollisionObject.transform(convexToLocal);
                // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
                Transform rotationXform = stack.allocTransform();
                Matrix3f tmpMat = stack.allocMatrix3f();
                tmpMat.mul(worldTocollisionObject.basis, convexToTrans.basis);
                rotationXform.set(tmpMat);
                BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, convexFromTrans, convexToTrans, resultCallback, collisionObject, triangleMesh, colObjWorldTransform);
                tccb.hitFraction = resultCallback.closestHitFraction;
                tccb.normalInWorldSpace = false;
                Vector3f boxMinLocal = stack.allocVector3f();
                Vector3f boxMaxLocal = stack.allocVector3f();
                castShape.getAabb(rotationXform, boxMinLocal, boxMaxLocal);
                Vector3f rayAabbMinLocal = stack.alloc(convexFromLocal);
                VectorUtil.setMin(rayAabbMinLocal, convexToLocal);
                Vector3f rayAabbMaxLocal = stack.alloc(convexFromLocal);
                VectorUtil.setMax(rayAabbMaxLocal, convexToLocal);
                rayAabbMinLocal.add(boxMinLocal);
                rayAabbMaxLocal.add(boxMaxLocal);
                triangleMesh.processAllTriangles(tccb, rayAabbMinLocal, rayAabbMaxLocal);
            }
        } else {
            // todo: use AABB tree or other BVH acceleration structure!
            if (collisionShape.isCompound()) {
                CompoundShape compoundShape = (CompoundShape) collisionShape;
                for (int i = 0; i < compoundShape.getNumChildShapes(); i++) {
                    Transform childTrans = compoundShape.getChildTransform(i, stack.allocTransform());
                    CollisionShape childCollisionShape = compoundShape.getChildShape(i);
                    Transform childWorldTrans = stack.allocTransform();
                    childWorldTrans.mul(colObjWorldTransform, childTrans);
                    // replace collision shape so that callback can determine the triangle
                    CollisionShape saveCollisionShape = collisionObject.getCollisionShape();
                    collisionObject.internalSetTemporaryCollisionShape(childCollisionShape);
                    objectQuerySingle(castShape, convexFromTrans, convexToTrans, collisionObject, childCollisionShape, childWorldTrans, resultCallback, allowedPenetration);
                    // restore
                    collisionObject.internalSetTemporaryCollisionShape(saveCollisionShape);
                }
            }
        }
    }
    stack.leave();
}

Example 3 with Matrix3f

use of javax.vecmath.Matrix3f in project bdx by GoranM.

the class RigidBody method updateInertiaTensor.

public void updateInertiaTensor() {
    Stack stack = Stack.enter();
    Matrix3f mat1 = stack.allocMatrix3f();
    MatrixUtil.scale(mat1, worldTransform.basis, invInertiaLocal);
    Matrix3f mat2 = stack.alloc(worldTransform.basis);
    mat2.transpose();
    invInertiaTensorWorld.mul(mat1, mat2);
    stack.leave();
}

Example 4 with Matrix3f

use of javax.vecmath.Matrix3f in project bdx by GoranM.

the class Generic6DofConstraint method buildAngularJacobian.

protected void buildAngularJacobian(/*JacobianEntry jacAngular*/
int jacAngular_index, Vector3f jointAxisW) {
    Stack stack = Stack.enter();
    Matrix3f mat1 = rbA.getCenterOfMassTransform(stack.allocTransform()).basis;
    mat1.transpose();
    Matrix3f mat2 = rbB.getCenterOfMassTransform(stack.allocTransform()).basis;
    mat2.transpose();
    jacAng[jacAngular_index].init(jointAxisW, mat1, mat2, rbA.getInvInertiaDiagLocal(stack.allocVector3f()), rbB.getInvInertiaDiagLocal(stack.allocVector3f()));
    stack.leave();
}

Example 5 with Matrix3f

use of javax.vecmath.Matrix3f in project bdx by GoranM.

the class Generic6DofConstraint method calculateAngleInfo.

/**
	 * Calcs the euler angles between the two bodies.
	 */
protected void calculateAngleInfo() {
    Stack stack = Stack.enter();
    Matrix3f mat = stack.allocMatrix3f();
    Matrix3f relative_frame = stack.allocMatrix3f();
    mat.set(calculatedTransformA.basis);
    MatrixUtil.invert(mat);
    relative_frame.mul(mat, calculatedTransformB.basis);
    matrixToEulerXYZ(relative_frame, calculatedAxisAngleDiff);
    // in euler angle mode we do not actually constrain the angular velocity
    // along the axes axis[0] and axis[2] (although we do use axis[1]) :
    //
    //    to get			constrain w2-w1 along		...not
    //    ------			---------------------		------
    //    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
    //    d(angle[1])/dt = 0	ax[1]
    //    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
    //
    // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
    // to prove the result for angle[0], write the expression for angle[0] from
    // GetInfo1 then take the derivative. to prove this for angle[2] it is
    // easier to take the euler rate expression for d(angle[2])/dt with respect
    // to the components of w and set that to 0.
    Vector3f axis0 = stack.allocVector3f();
    calculatedTransformB.basis.getColumn(0, axis0);
    Vector3f axis2 = stack.allocVector3f();
    calculatedTransformA.basis.getColumn(2, axis2);
    calculatedAxis[1].cross(axis2, axis0);
    calculatedAxis[0].cross(calculatedAxis[1], axis2);
    calculatedAxis[2].cross(axis0, calculatedAxis[1]);
    //    if(m_debugDrawer)
    //    {
    //
    //    	char buff[300];
    //		sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
    //		m_calculatedAxisAngleDiff[0],
    //		m_calculatedAxisAngleDiff[1],
    //		m_calculatedAxisAngleDiff[2]);
    //    	m_debugDrawer->reportErrorWarning(buff);
    //    }
    stack.leave();
}