Examples with SimpleMatrix org.ejml.simple.SimpleMatrix used on opensource projects

Example 86 with SimpleMatrix

use of org.ejml.simple.SimpleMatrix in project jmonkeyengine by jMonkeyEngine.

the class ConstraintDefinitionIK method bake.

@Override
public void bake(Space ownerSpace, Space targetSpace, Transform targetTransform, float influence) {
    if (influence == 0 || !trackToBeChanged || targetTransform == null || bonesCount == 0) {
        // no need to do anything
        return;
    }
    if (bones == null) {
        bones = new BonesChain((Bone) this.getOwner(), useTail, bonesAffected, alteredOmas, blenderContext);
    }
    if (bones.size() == 0) {
        bonesCount = 0;
        // no need to do anything
        return;
    }
    double distanceFromTarget = Double.MAX_VALUE;
    target.set(targetTransform.getTranslation().x, targetTransform.getTranslation().y, targetTransform.getTranslation().z);
    if (bonesCount < 0) {
        bonesCount = bones.size();
        rotationVectors = new Vector3d[bonesCount];
        for (int i = 0; i < bonesCount; ++i) {
            rotationVectors[i] = new Vector3d();
        }
        J = new Matrix(3, bonesCount);
    }
    BoneContext topBone = bones.get(0);
    for (int i = 0; i < iterations; ++i) {
        DTransform topBoneTransform = bones.getWorldTransform(topBone);
        // effector
        Vector3d e = topBoneTransform.getTranslation().add(topBoneTransform.getRotation().mult(Vector3d.UNIT_Y).multLocal(topBone.getLength()));
        distanceFromTarget = e.distance(target);
        if (distanceFromTarget <= MIN_DISTANCE) {
            break;
        }
        deltaP.setColumn(0, 0, target.x - e.x, target.y - e.y, target.z - e.z);
        int column = 0;
        for (BoneContext boneContext : bones) {
            DTransform boneWorldTransform = bones.getWorldTransform(boneContext);
            // current join position
            Vector3d j = boneWorldTransform.getTranslation();
            Vector3d vectorFromJointToEffector = e.subtract(j);
            vectorFromJointToEffector.cross(target.subtract(j), rotationVectors[column]).normalizeLocal();
            rotationVectors[column].cross(vectorFromJointToEffector, col);
            J.setColumn(col, column++);
        }
        Matrix J_1 = J.pseudoinverse();
        SimpleMatrix deltaThetas = J_1.mult(deltaP);
        if (deltaThetas.elementMaxAbs() < MIN_ANGLE_CHANGE) {
            break;
        }
        for (int j = 0; j < deltaThetas.numRows(); ++j) {
            double angle = deltaThetas.get(j, 0);
            Vector3d rotationVector = rotationVectors[j];
            tempDQuaternion.fromAngleAxis(angle, rotationVector);
            BoneContext boneContext = bones.get(j);
            Bone bone = boneContext.getBone();
            if (bone.equals(this.getOwner())) {
                if (boneContext.isLockX()) {
                    tempDQuaternion.set(0, tempDQuaternion.getY(), tempDQuaternion.getZ(), tempDQuaternion.getW());
                }
                if (boneContext.isLockY()) {
                    tempDQuaternion.set(tempDQuaternion.getX(), 0, tempDQuaternion.getZ(), tempDQuaternion.getW());
                }
                if (boneContext.isLockZ()) {
                    tempDQuaternion.set(tempDQuaternion.getX(), tempDQuaternion.getY(), 0, tempDQuaternion.getW());
                }
            }
            DTransform boneTransform = bones.getWorldTransform(boneContext);
            boneTransform.getRotation().set(tempDQuaternion.mult(boneTransform.getRotation()));
            bones.setWorldTransform(boneContext, boneTransform);
        }
    }
    // applying the results
    for (int i = bonesCount - 1; i >= 0; --i) {
        BoneContext boneContext = bones.get(i);
        DTransform transform = bones.getWorldTransform(boneContext);
        constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD, transform.toTransform());
    }
    // need to reload them again
    bones = null;
}

Example 87 with SimpleMatrix

use of org.ejml.simple.SimpleMatrix in project jmonkeyengine by jMonkeyEngine.

the class Matrix method pseudoinverse.

@SuppressWarnings("unchecked")
public Matrix pseudoinverse(double lambda) {
    SimpleSVD<SimpleMatrix> simpleSVD = this.svd();
    SimpleMatrix U = simpleSVD.getU();
    SimpleMatrix S = simpleSVD.getW();
    SimpleMatrix V = simpleSVD.getV();
    int N = Math.min(this.numRows(), this.numCols());
    double maxSingular = 0;
    for (int i = 0; i < N; ++i) {
        if (S.get(i, i) > maxSingular) {
            maxSingular = S.get(i, i);
        }
    }
    double tolerance = FastMath.DBL_EPSILON * Math.max(this.numRows(), this.numCols()) * maxSingular;
    for (int i = 0; i < Math.min(S.numRows(), S.numCols()); ++i) {
        double a = S.get(i, i);
        if (a <= tolerance) {
            a = 0;
        } else {
            a = a / (a * a + lambda * lambda);
        }
        S.set(i, i, a);
    }
    return new Matrix(V.mult(S.transpose()).mult(U.transpose()));
}

Example 88 with SimpleMatrix

use of org.ejml.simple.SimpleMatrix in project BoofCV by lessthanoptimal.

the class ImplRectifyImageOps_F32 method adjustUncalibrated.

/**
 * Internal function which applies the rectification adjustment to an uncalibrated stereo pair
 */
private static void adjustUncalibrated(FMatrixRMaj rectifyLeft, FMatrixRMaj rectifyRight, RectangleLength2D_F32 bound, float scale) {
    // translation
    float deltaX = -bound.x0 * scale;
    float deltaY = -bound.y0 * scale;
    // adjustment matrix
    SimpleMatrix A = new SimpleMatrix(3, 3, true, new float[] { scale, 0, deltaX, 0, scale, deltaY, 0, 0, 1 });
    SimpleMatrix rL = SimpleMatrix.wrap(rectifyLeft);
    SimpleMatrix rR = SimpleMatrix.wrap(rectifyRight);
    rectifyLeft.set(A.mult(rL).getFDRM());
    rectifyRight.set(A.mult(rR).getFDRM());
}

Example 89 with SimpleMatrix

use of org.ejml.simple.SimpleMatrix in project Java-Matrix-Benchmark by lessthanoptimal.

the class EjmlSimpleAlgorithmFactory method convertToRowMajor.

@Override
public RowMajorMatrix convertToRowMajor(BenchmarkMatrix input) {
    SimpleMatrix m = input.getOriginal();
    RowMajorMatrix out = new RowMajorMatrix(1, 1);
    out.data = m.getDDRM().data;
    out.numCols = m.getDDRM().numCols;
    out.numRows = m.getDDRM().numRows;
    return out;
}

Example 90 with SimpleMatrix

use of org.ejml.simple.SimpleMatrix in project froog by mroodschild.

the class Softmax method output.

/**
 * la salida será:<br>
 * <table>
 * <tr><td></td><td>[ 1
 * ]</td><td></td><td>[e<sup>(z<sub>1</sub>)</sup>]</td></tr>
 * <tr><td>a =
 * </td><td>------</td><td>*</td><td>[e<sup>(z<sub>2</sub>)</sup>]</td></tr>
 * <tr><td></td><td>&sum(e<sup>zi</sup>)</td><td></td><td>[e<sup>(z<sub>3</sub>)</sup>]</td></tr>
 * </table>
 *
 * @param z
 * @return
 */
@Override
public SimpleMatrix output(SimpleMatrix z) {
    // SimpleMatrix p = z.elementExp();
    // double sum = p.elementSum();
    // return p.divide(sum);
    SimpleMatrix p = z.elementExp();
    SimpleMatrix sum = new SimpleMatrix(1, p.numCols());
    CommonOps.sumCols(p.getMatrix(), sum.getMatrix());
    for (int i = 0; i < p.numCols(); i++) {
        p.setColumn(i, 0, p.extractVector(false, i).divide(sum.get(i)).getMatrix().getData());
    }
    return p;
}