Examples with Matrix4f com.jme3.math.Matrix4f used on opensource projects

Example 26 with Matrix4f

use of com.jme3.math.Matrix4f in project jmonkeyengine by jMonkeyEngine.

the class BIHNode method intersectWhere.

public final int intersectWhere(Ray r, Matrix4f worldMatrix, BIHTree tree, float sceneMin, float sceneMax, CollisionResults results) {
    TempVars vars = TempVars.get();
    ArrayList<BIHStackData> stack = vars.bihStack;
    stack.clear();
    //        float tHit = Float.POSITIVE_INFINITY;
    Vector3f o = vars.vect1.set(r.getOrigin());
    Vector3f d = vars.vect2.set(r.getDirection());
    Matrix4f inv = vars.tempMat4.set(worldMatrix).invertLocal();
    inv.mult(r.getOrigin(), r.getOrigin());
    // Fixes rotation collision bug
    inv.multNormal(r.getDirection(), r.getDirection());
    //        inv.multNormalAcross(r.getDirection(), r.getDirection());
    float[] origins = { r.getOrigin().x, r.getOrigin().y, r.getOrigin().z };
    float[] invDirections = { 1f / r.getDirection().x, 1f / r.getDirection().y, 1f / r.getDirection().z };
    r.getDirection().normalizeLocal();
    Vector3f v1 = vars.vect3, v2 = vars.vect4, v3 = vars.vect5;
    int cols = 0;
    stack.add(new BIHStackData(this, sceneMin, sceneMax));
    stackloop: while (stack.size() > 0) {
        BIHStackData data = stack.remove(stack.size() - 1);
        BIHNode node = data.node;
        float tMin = data.min, tMax = data.max;
        if (tMax < tMin) {
            continue;
        }
        leafloop: while (node.axis != 3) {
            // while node is not a leaf
            int a = node.axis;
            // find the origin and direction value for the given axis
            float origin = origins[a];
            float invDirection = invDirections[a];
            float tNearSplit, tFarSplit;
            BIHNode nearNode, farNode;
            tNearSplit = (node.leftPlane - origin) * invDirection;
            tFarSplit = (node.rightPlane - origin) * invDirection;
            nearNode = node.left;
            farNode = node.right;
            if (invDirection < 0) {
                float tmpSplit = tNearSplit;
                tNearSplit = tFarSplit;
                tFarSplit = tmpSplit;
                BIHNode tmpNode = nearNode;
                nearNode = farNode;
                farNode = tmpNode;
            }
            if (tMin > tNearSplit && tMax < tFarSplit) {
                continue stackloop;
            }
            if (tMin > tNearSplit) {
                tMin = max(tMin, tFarSplit);
                node = farNode;
            } else if (tMax < tFarSplit) {
                tMax = min(tMax, tNearSplit);
                node = nearNode;
            } else {
                stack.add(new BIHStackData(farNode, max(tMin, tFarSplit), tMax));
                tMax = min(tMax, tNearSplit);
                node = nearNode;
            }
        }
        // a leaf
        for (int i = node.leftIndex; i <= node.rightIndex; i++) {
            tree.getTriangle(i, v1, v2, v3);
            float t = r.intersects(v1, v2, v3);
            if (!Float.isInfinite(t)) {
                if (worldMatrix != null) {
                    worldMatrix.mult(v1, v1);
                    worldMatrix.mult(v2, v2);
                    worldMatrix.mult(v3, v3);
                    float t_world = new Ray(o, d).intersects(v1, v2, v3);
                    t = t_world;
                }
                Vector3f contactNormal = Triangle.computeTriangleNormal(v1, v2, v3, null);
                Vector3f contactPoint = new Vector3f(d).multLocal(t).addLocal(o);
                float worldSpaceDist = o.distance(contactPoint);
                CollisionResult cr = new CollisionResult(contactPoint, worldSpaceDist);
                cr.setContactNormal(contactNormal);
                cr.setTriangleIndex(tree.getTriangleIndex(i));
                results.addCollision(cr);
                cols++;
            }
        }
    }
    vars.release();
    r.setOrigin(o);
    r.setDirection(d);
    return cols;
}

Example 27 with Matrix4f

use of com.jme3.math.Matrix4f in project jmonkeyengine by jMonkeyEngine.

the class BIHNode method intersectBrute.

public final int intersectBrute(Ray r, Matrix4f worldMatrix, BIHTree tree, float sceneMin, float sceneMax, CollisionResults results) {
    float tHit = Float.POSITIVE_INFINITY;
    TempVars vars = TempVars.get();
    Vector3f v1 = vars.vect1, v2 = vars.vect2, v3 = vars.vect3;
    int cols = 0;
    ArrayList<BIHStackData> stack = vars.bihStack;
    stack.clear();
    stack.add(new BIHStackData(this, 0, 0));
    stackloop: while (stack.size() > 0) {
        BIHStackData data = stack.remove(stack.size() - 1);
        BIHNode node = data.node;
        leafloop: while (node.axis != 3) {
            // while node is not a leaf
            BIHNode nearNode, farNode;
            nearNode = node.left;
            farNode = node.right;
            stack.add(new BIHStackData(farNode, 0, 0));
            node = nearNode;
        }
        // a leaf
        for (int i = node.leftIndex; i <= node.rightIndex; i++) {
            tree.getTriangle(i, v1, v2, v3);
            if (worldMatrix != null) {
                worldMatrix.mult(v1, v1);
                worldMatrix.mult(v2, v2);
                worldMatrix.mult(v3, v3);
            }
            float t = r.intersects(v1, v2, v3);
            if (t < tHit) {
                tHit = t;
                Vector3f contactPoint = new Vector3f(r.direction).multLocal(tHit).addLocal(r.origin);
                CollisionResult cr = new CollisionResult(contactPoint, tHit);
                cr.setTriangleIndex(tree.getTriangleIndex(i));
                results.addCollision(cr);
                cols++;
            }
        }
    }
    vars.release();
    return cols;
}

Example 28 with Matrix4f

use of com.jme3.math.Matrix4f in project jmonkeyengine by jMonkeyEngine.

the class GeometryBatchFactory method mergeGeometries.

/**
     * Merges all geometries in the collection into
     * the output mesh. Creates a new material using the TextureAtlas.
     * 
     * @param geometries
     * @param outMesh
     */
public static void mergeGeometries(Collection<Geometry> geometries, Mesh outMesh) {
    int[] compsForBuf = new int[VertexBuffer.Type.values().length];
    Format[] formatForBuf = new Format[compsForBuf.length];
    boolean[] normForBuf = new boolean[VertexBuffer.Type.values().length];
    int totalVerts = 0;
    int totalTris = 0;
    int totalLodLevels = 0;
    int maxWeights = -1;
    Mode mode = null;
    for (Geometry geom : geometries) {
        totalVerts += geom.getVertexCount();
        totalTris += geom.getTriangleCount();
        totalLodLevels = Math.min(totalLodLevels, geom.getMesh().getNumLodLevels());
        Mode listMode;
        int components;
        switch(geom.getMesh().getMode()) {
            case Points:
                listMode = Mode.Points;
                components = 0;
                break;
            case LineLoop:
            case LineStrip:
            case Lines:
                listMode = Mode.Lines;
                components = 2;
                break;
            case TriangleFan:
            case TriangleStrip:
            case Triangles:
                listMode = Mode.Triangles;
                components = 3;
                break;
            default:
                throw new UnsupportedOperationException();
        }
        for (VertexBuffer vb : geom.getMesh().getBufferList().getArray()) {
            int currentCompsForBuf = compsForBuf[vb.getBufferType().ordinal()];
            if (vb.getBufferType() != Type.Index && currentCompsForBuf != 0 && currentCompsForBuf != vb.getNumComponents()) {
                throw new UnsupportedOperationException("The geometry " + geom + " buffer " + vb.getBufferType() + " has different number of components than the rest of the meshes " + "(this: " + vb.getNumComponents() + ", expected: " + currentCompsForBuf + ")");
            }
            compsForBuf[vb.getBufferType().ordinal()] = vb.getNumComponents();
            formatForBuf[vb.getBufferType().ordinal()] = vb.getFormat();
            normForBuf[vb.getBufferType().ordinal()] = vb.isNormalized();
        }
        maxWeights = Math.max(maxWeights, geom.getMesh().getMaxNumWeights());
        if (mode != null && mode != listMode) {
            throw new UnsupportedOperationException("Cannot combine different" + " primitive types: " + mode + " != " + listMode);
        }
        mode = listMode;
        compsForBuf[Type.Index.ordinal()] = components;
    }
    outMesh.setMaxNumWeights(maxWeights);
    outMesh.setMode(mode);
    if (totalVerts >= 65536) {
        // make sure we create an UnsignedInt buffer so
        // we can fit all of the meshes
        formatForBuf[Type.Index.ordinal()] = Format.UnsignedInt;
    } else {
        formatForBuf[Type.Index.ordinal()] = Format.UnsignedShort;
    }
    // generate output buffers based on retrieved info
    for (int i = 0; i < compsForBuf.length; i++) {
        if (compsForBuf[i] == 0) {
            continue;
        }
        Buffer data;
        if (i == Type.Index.ordinal()) {
            data = VertexBuffer.createBuffer(formatForBuf[i], compsForBuf[i], totalTris);
        } else {
            data = VertexBuffer.createBuffer(formatForBuf[i], compsForBuf[i], totalVerts);
        }
        VertexBuffer vb = new VertexBuffer(Type.values()[i]);
        vb.setupData(Usage.Static, compsForBuf[i], formatForBuf[i], data);
        vb.setNormalized(normForBuf[i]);
        outMesh.setBuffer(vb);
    }
    int globalVertIndex = 0;
    int globalTriIndex = 0;
    for (Geometry geom : geometries) {
        Mesh inMesh = geom.getMesh();
        geom.computeWorldMatrix();
        Matrix4f worldMatrix = geom.getWorldMatrix();
        int geomVertCount = inMesh.getVertexCount();
        int geomTriCount = inMesh.getTriangleCount();
        for (int bufType = 0; bufType < compsForBuf.length; bufType++) {
            VertexBuffer inBuf = inMesh.getBuffer(Type.values()[bufType]);
            VertexBuffer outBuf = outMesh.getBuffer(Type.values()[bufType]);
            if (inBuf == null || outBuf == null) {
                continue;
            }
            if (Type.Index.ordinal() == bufType) {
                int components = compsForBuf[bufType];
                IndexBuffer inIdx = inMesh.getIndicesAsList();
                IndexBuffer outIdx = outMesh.getIndexBuffer();
                for (int tri = 0; tri < geomTriCount; tri++) {
                    for (int comp = 0; comp < components; comp++) {
                        int idx = inIdx.get(tri * components + comp) + globalVertIndex;
                        outIdx.put((globalTriIndex + tri) * components + comp, idx);
                    }
                }
            } else if (Type.Position.ordinal() == bufType) {
                FloatBuffer inPos = (FloatBuffer) inBuf.getDataReadOnly();
                FloatBuffer outPos = (FloatBuffer) outBuf.getData();
                doTransformVerts(inPos, globalVertIndex, outPos, worldMatrix);
            } else if (Type.Normal.ordinal() == bufType) {
                FloatBuffer inPos = (FloatBuffer) inBuf.getDataReadOnly();
                FloatBuffer outPos = (FloatBuffer) outBuf.getData();
                doTransformNorms(inPos, globalVertIndex, outPos, worldMatrix);
            } else if (Type.Tangent.ordinal() == bufType) {
                FloatBuffer inPos = (FloatBuffer) inBuf.getDataReadOnly();
                FloatBuffer outPos = (FloatBuffer) outBuf.getData();
                int components = inBuf.getNumComponents();
                doTransformTangents(inPos, globalVertIndex, components, outPos, worldMatrix);
            } else {
                inBuf.copyElements(0, outBuf, globalVertIndex, geomVertCount);
            }
        }
        globalVertIndex += geomVertCount;
        globalTriIndex += geomTriCount;
    }
}

Example 29 with Matrix4f

use of com.jme3.math.Matrix4f in project jmonkeyengine by jMonkeyEngine.

the class BoneContext method buildBone.

/**
     * This method builds the bone. It recursively builds the bone's children.
     * 
     * @param bones
     *            a list of bones where the newly created bone will be added
     * @param skeletonOwnerOma
     *            the spatial of the object that will own the skeleton
     * @param blenderContext
     *            the blender context
     * @return newly created bone
     */
public Bone buildBone(List<Bone> bones, Long skeletonOwnerOma, BlenderContext blenderContext) {
    this.skeletonOwnerOma = skeletonOwnerOma;
    Long boneOMA = boneStructure.getOldMemoryAddress();
    bone = new Bone(boneName);
    bones.add(bone);
    blenderContext.addLoadedFeatures(boneOMA, LoadedDataType.STRUCTURE, boneStructure);
    blenderContext.addLoadedFeatures(boneOMA, LoadedDataType.FEATURE, bone);
    ObjectHelper objectHelper = blenderContext.getHelper(ObjectHelper.class);
    Structure skeletonOwnerObjectStructure = (Structure) blenderContext.getLoadedFeature(skeletonOwnerOma, LoadedDataType.STRUCTURE);
    // I could load 'imat' here, but apparently in some older blenders there were bugs or unfinished functionalities that stored ZERO matrix in imat field
    // loading 'obmat' and inverting it makes us avoid errors in such cases
    Matrix4f invertedObjectOwnerGlobalMatrix = objectHelper.getMatrix(skeletonOwnerObjectStructure, "obmat", blenderContext.getBlenderKey().isFixUpAxis()).invertLocal();
    if (objectHelper.isParent(skeletonOwnerOma, armatureObjectOMA)) {
        boneMatrixInModelSpace = globalBoneMatrix.mult(invertedObjectOwnerGlobalMatrix);
    } else {
        boneMatrixInModelSpace = invertedObjectOwnerGlobalMatrix.mult(globalBoneMatrix);
    }
    Matrix4f boneLocalMatrix = parent == null ? boneMatrixInModelSpace : parent.boneMatrixInModelSpace.invert().multLocal(boneMatrixInModelSpace);
    Vector3f poseLocation = parent == null || !this.is(CONNECTED_TO_PARENT) ? boneLocalMatrix.toTranslationVector() : new Vector3f(0, parent.length, 0);
    Quaternion rotation = boneLocalMatrix.toRotationQuat().normalizeLocal();
    Vector3f scale = boneLocalMatrix.toScaleVector();
    bone.setBindTransforms(poseLocation, rotation, scale);
    for (BoneContext child : children) {
        bone.addChild(child.buildBone(bones, skeletonOwnerOma, blenderContext));
    }
    return bone;
}

Example 30 with Matrix4f

use of com.jme3.math.Matrix4f in project jmonkeyengine by jMonkeyEngine.

the class ConstraintHelper method applyTransform.

/**
     * Applies transform to a feature (bone or spatial). Computations transform
     * the given transformation from the given space to the feature's local
     * space.
     * 
     * @param oma
     *            the OMA of the feature we apply transformation to
     * @param subtargetName
     *            the name of the feature's subtarget (bone in case of armature)
     * @param space
     *            the space in which the given transform is to be applied
     * @param transform
     *            the transform we apply
     */
public void applyTransform(Long oma, String subtargetName, Space space, Transform transform) {
    Spatial feature = (Spatial) blenderContext.getLoadedFeature(oma, LoadedDataType.FEATURE);
    boolean isArmature = blenderContext.getMarkerValue(ObjectHelper.ARMATURE_NODE_MARKER, feature) != null;
    if (isArmature) {
        Skeleton skeleton = blenderContext.getSkeleton(oma);
        BoneContext targetBoneContext = blenderContext.getBoneByName(oma, subtargetName);
        Bone bone = targetBoneContext.getBone();
        if (bone.getParent() == null && (space == Space.CONSTRAINT_SPACE_LOCAL || space == Space.CONSTRAINT_SPACE_PARLOCAL)) {
            space = Space.CONSTRAINT_SPACE_POSE;
        }
        TempVars tempVars = TempVars.get();
        switch(space) {
            case CONSTRAINT_SPACE_LOCAL:
                assert bone.getParent() != null : "CONSTRAINT_SPACE_LOCAL should be evaluated as CONSTRAINT_SPACE_POSE if the bone has no parent!";
                bone.setBindTransforms(transform.getTranslation(), transform.getRotation(), transform.getScale());
                break;
            case CONSTRAINT_SPACE_WORLD:
                {
                    Matrix4f boneMatrixInWorldSpace = this.toMatrix(transform, tempVars.tempMat4);
                    Matrix4f modelWorldMatrix = this.toMatrix(this.getTransform(targetBoneContext.getSkeletonOwnerOma(), null, Space.CONSTRAINT_SPACE_WORLD), tempVars.tempMat42);
                    Matrix4f boneMatrixInModelSpace = modelWorldMatrix.invertLocal().multLocal(boneMatrixInWorldSpace);
                    Bone parent = bone.getParent();
                    if (parent != null) {
                        Matrix4f parentMatrixInModelSpace = this.toMatrix(parent.getModelSpacePosition(), parent.getModelSpaceRotation(), parent.getModelSpaceScale(), tempVars.tempMat4);
                        boneMatrixInModelSpace = parentMatrixInModelSpace.invertLocal().multLocal(boneMatrixInModelSpace);
                    }
                    bone.setBindTransforms(boneMatrixInModelSpace.toTranslationVector(), boneMatrixInModelSpace.toRotationQuat(), boneMatrixInModelSpace.toScaleVector());
                    break;
                }
            case CONSTRAINT_SPACE_POSE:
                {
                    Matrix4f armatureWorldMatrix = this.toMatrix(feature.getWorldTransform(), tempVars.tempMat4);
                    Matrix4f boneMatrixInWorldSpace = armatureWorldMatrix.multLocal(this.toMatrix(transform, tempVars.tempMat42));
                    Matrix4f invertedModelMatrix = this.toMatrix(this.getTransform(targetBoneContext.getSkeletonOwnerOma(), null, Space.CONSTRAINT_SPACE_WORLD), tempVars.tempMat42).invertLocal();
                    Matrix4f boneMatrixInModelSpace = invertedModelMatrix.multLocal(boneMatrixInWorldSpace);
                    Bone parent = bone.getParent();
                    if (parent != null) {
                        Matrix4f parentMatrixInModelSpace = this.toMatrix(parent.getModelSpacePosition(), parent.getModelSpaceRotation(), parent.getModelSpaceScale(), tempVars.tempMat4);
                        boneMatrixInModelSpace = parentMatrixInModelSpace.invertLocal().multLocal(boneMatrixInModelSpace);
                    }
                    bone.setBindTransforms(boneMatrixInModelSpace.toTranslationVector(), boneMatrixInModelSpace.toRotationQuat(), boneMatrixInModelSpace.toScaleVector());
                    break;
                }
            case CONSTRAINT_SPACE_PARLOCAL:
                Matrix4f armatureWorldMatrix = this.toMatrix(feature.getWorldTransform(), tempVars.tempMat4);
                Matrix4f boneMatrixInWorldSpace = armatureWorldMatrix.multLocal(this.toMatrix(transform, tempVars.tempMat42));
                Matrix4f invertedModelMatrix = this.toMatrix(this.getTransform(targetBoneContext.getSkeletonOwnerOma(), null, Space.CONSTRAINT_SPACE_WORLD), tempVars.tempMat42).invertLocal();
                Matrix4f boneMatrixInModelSpace = invertedModelMatrix.multLocal(boneMatrixInWorldSpace);
                Bone parent = bone.getParent();
                if (parent != null) {
                    //first add the initial parent matrix to the bone's model matrix
                    BoneContext parentContext = blenderContext.getBoneContext(parent);
                    Matrix4f initialParentMatrixInModelSpace = parentContext.getBoneMatrixInModelSpace();
                    Matrix4f currentParentMatrixInModelSpace = this.toMatrix(parent.getModelSpacePosition(), parent.getModelSpaceRotation(), parent.getModelSpaceScale(), tempVars.tempMat4);
                    //the bone will now move with its parent in model space
                    //now we need to subtract the difference between current parent's model matrix and its initial model matrix
                    boneMatrixInModelSpace = initialParentMatrixInModelSpace.mult(boneMatrixInModelSpace);
                    Matrix4f diffMatrix = initialParentMatrixInModelSpace.mult(currentParentMatrixInModelSpace.invert());
                    boneMatrixInModelSpace.multLocal(diffMatrix);
                //now the bone will have its position in model space with initial parent's model matrix added
                }
                bone.setBindTransforms(boneMatrixInModelSpace.toTranslationVector(), boneMatrixInModelSpace.toRotationQuat(), boneMatrixInModelSpace.toScaleVector());
                break;
            default:
                tempVars.release();
                throw new IllegalStateException("Invalid space type for target object: " + space.toString());
        }
        tempVars.release();
        skeleton.updateWorldVectors();
    } else {
        switch(space) {
            case CONSTRAINT_SPACE_LOCAL:
                feature.getLocalTransform().set(transform);
                break;
            case CONSTRAINT_SPACE_WORLD:
                if (feature.getParent() == null) {
                    feature.setLocalTransform(transform);
                } else {
                    Transform parentWorldTransform = feature.getParent().getWorldTransform();
                    TempVars tempVars = TempVars.get();
                    Matrix4f parentInverseMatrix = this.toMatrix(parentWorldTransform, tempVars.tempMat4).invertLocal();
                    Matrix4f m = this.toMatrix(transform, tempVars.tempMat42);
                    m = m.multLocal(parentInverseMatrix);
                    tempVars.release();
                    transform.setTranslation(m.toTranslationVector());
                    transform.setRotation(m.toRotationQuat());
                    transform.setScale(m.toScaleVector());
                    feature.setLocalTransform(transform);
                }
                break;
            default:
                throw new IllegalStateException("Invalid space type for spatial object: " + space.toString());
        }
    }
}