Examples with Transform spacegraph.space3d.phys.math.Transform used on opensource projects

Example 41 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class Point2PointConstraint method buildJacobian.

@Override
public void buildJacobian() {
    appliedImpulse = 0f;
    v3 normal = new v3();
    normal.set(0f, 0f, 0f);
    Matrix3f tmpMat1 = new Matrix3f();
    Matrix3f tmpMat2 = new Matrix3f();
    v3 tmp1 = new v3();
    v3 tmp2 = new v3();
    v3 tmpVec = new v3();
    Transform centerOfMassA = rbA.getCenterOfMassTransform(new Transform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(new Transform());
    for (int i = 0; i < 3; i++) {
        VectorUtil.setCoord(normal, i, 1f);
        tmpMat1.transpose(centerOfMassA.basis);
        tmpMat2.transpose(centerOfMassB.basis);
        tmp1.set(pivotInA);
        centerOfMassA.transform(tmp1);
        tmp1.sub(rbA.getCenterOfMassPosition(tmpVec));
        tmp2.set(pivotInB);
        centerOfMassB.transform(tmp2);
        tmp2.sub(rbB.getCenterOfMassPosition(tmpVec));
        jac[i].init(tmpMat1, tmpMat2, tmp1, tmp2, normal, rbA.getInvInertiaDiagLocal(new v3()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(new v3()), rbB.getInvMass());
        VectorUtil.setCoord(normal, i, 0f);
    }
}

Example 42 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class Generic6DofConstraint method buildLinearJacobian.

protected void buildLinearJacobian(/*JacobianEntry jacLinear*/
int jacLinear_index, v3 normalWorld, v3 pivotAInW, v3 pivotBInW) {
    Matrix3f mat1 = rbA.getCenterOfMassTransform(new Transform()).basis;
    mat1.transpose();
    Matrix3f mat2 = rbB.getCenterOfMassTransform(new Transform()).basis;
    mat2.transpose();
    v3 tmpVec = new v3();
    v3 tmp1 = new v3();
    tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
    v3 tmp2 = new v3();
    tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
    jacLinear[jacLinear_index].init(mat1, mat2, tmp1, tmp2, normalWorld, rbA.getInvInertiaDiagLocal(new v3()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(new v3()), rbB.getInvMass());
}

Example 43 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class Generic6DofConstraint method buildAngularJacobian.

protected void buildAngularJacobian(/*JacobianEntry jacAngular*/
int jacAngular_index, v3 jointAxisW) {
    Matrix3f mat1 = rbA.getCenterOfMassTransform(new Transform()).basis;
    mat1.transpose();
    Matrix3f mat2 = rbB.getCenterOfMassTransform(new Transform()).basis;
    mat2.transpose();
    jacAng[jacAngular_index].init(jointAxisW, mat1, mat2, rbA.getInvInertiaDiagLocal(new v3()), rbB.getInvInertiaDiagLocal(new v3()));
}

Example 44 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class ConeShape method calculateLocalInertia.

@Override
public void calculateLocalInertia(float mass, v3 inertia) {
    Transform identity = new Transform();
    identity.setIdentity();
    v3 aabbMin = new v3(), aabbMax = new v3();
    getAabb(identity, aabbMin, aabbMax);
    v3 halfExtents = new v3();
    halfExtents.sub(aabbMax, aabbMin);
    halfExtents.scale(0.5f);
    float margin = getMargin();
    float lx = 2f * (halfExtents.x + margin);
    float ly = 2f * (halfExtents.y + margin);
    float lz = 2f * (halfExtents.z + margin);
    float x2 = lx * lx;
    float y2 = ly * ly;
    float z2 = lz * lz;
    float scaledmass = mass * 0.08333333f;
    inertia.set(y2 + z2, x2 + z2, x2 + y2);
    inertia.scale(scaledmass);
// inertia.x() = scaledmass * (y2+z2);
// inertia.y() = scaledmass * (x2+z2);
// inertia.z() = scaledmass * (x2+y2);
}

Example 45 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class GjkPairDetector method getClosestPoints.

@Override
public void getClosestPoints(ClosestPointInput input, Result output, boolean swapResults) {
    v3 tmp = new v3();
    float distance = 0f;
    v3 normalInB = new v3();
    normalInB.set(0f, 0f, 0f);
    v3 pointOnA = new v3(), pointOnB = new v3();
    Transform localTransA = new Transform(input.transformA);
    Transform localTransB = new Transform(input.transformB);
    v3 positionOffset = new v3();
    positionOffset.add(localTransA, localTransB);
    positionOffset.scale(0.5f);
    localTransA.sub(positionOffset);
    localTransB.sub(positionOffset);
    float marginA = minkowskiA.getMargin();
    float marginB = minkowskiB.getMargin();
    BulletStats.gNumGjkChecks++;
    // for CCD we don't use margins
    if (ignoreMargin) {
        marginA = 0f;
        marginB = 0f;
    }
    curIter = 0;
    // this is to catch invalid input, perhaps check for #NaN?
    int gGjkMaxIter = 1000;
    cachedSeparatingAxis.set(0f, 1f, 0f);
    boolean isValid = false;
    boolean checkSimplex = false;
    boolean checkPenetration = true;
    degenerateSimplex = 0;
    lastUsedMethod = -1;
    float squaredDistance = BulletGlobals.SIMD_INFINITY;
    float delta = 0f;
    float margin = marginA + marginB;
    simplexSolver.reset();
    v3 seperatingAxisInA = new v3();
    v3 seperatingAxisInB = new v3();
    v3 pInA = new v3();
    v3 qInB = new v3();
    v3 pWorld = new v3();
    v3 qWorld = new v3();
    v3 w = new v3();
    v3 tmpPointOnA = new v3(), tmpPointOnB = new v3();
    v3 tmpNormalInB = new v3();
    for (; ; ) // while (true)
    {
        seperatingAxisInA.negate(cachedSeparatingAxis);
        MatrixUtil.transposeTransform(seperatingAxisInA, seperatingAxisInA, input.transformA.basis);
        seperatingAxisInB.set(cachedSeparatingAxis);
        MatrixUtil.transposeTransform(seperatingAxisInB, seperatingAxisInB, input.transformB.basis);
        minkowskiA.localGetSupportingVertexWithoutMargin(seperatingAxisInA, pInA);
        minkowskiB.localGetSupportingVertexWithoutMargin(seperatingAxisInB, qInB);
        pWorld.set(pInA);
        localTransA.transform(pWorld);
        qWorld.set(qInB);
        localTransB.transform(qWorld);
        w.sub(pWorld, qWorld);
        delta = cachedSeparatingAxis.dot(w);
        // potential exit, they don't overlap
        if ((delta > 0f) && (delta * delta > squaredDistance * input.maximumDistanceSquared)) {
            checkPenetration = false;
            break;
        }
        // exit 0: the new point is already in the simplex, or we didn't come any closer
        if (simplexSolver.inSimplex(w)) {
            degenerateSimplex = 1;
            checkSimplex = true;
            break;
        }
        // are we getting any closer ?
        float f0 = squaredDistance - delta;
        float f1 = squaredDistance * REL_ERROR2;
        if (f0 <= f1) {
            if (f0 <= 0f) {
                degenerateSimplex = 2;
            }
            checkSimplex = true;
            break;
        }
        // add current vertex to simplex
        simplexSolver.addVertex(w, pWorld, qWorld);
        // calculate the closest point to the origin (update vector v)
        if (!simplexSolver.closest(cachedSeparatingAxis)) {
            degenerateSimplex = 3;
            checkSimplex = true;
            break;
        }
        if (cachedSeparatingAxis.lengthSquared() < REL_ERROR2) {
            degenerateSimplex = 6;
            checkSimplex = true;
            break;
        }
        float previousSquaredDistance = squaredDistance;
        squaredDistance = cachedSeparatingAxis.lengthSquared();
        // are we getting any closer ?
        if (previousSquaredDistance - squaredDistance <= BulletGlobals.FLT_EPSILON * previousSquaredDistance) {
            simplexSolver.backup_closest(cachedSeparatingAxis);
            checkSimplex = true;
            break;
        }
        // degeneracy, this is typically due to invalid/uninitialized worldtransforms for a CollisionObject
        if (curIter++ > gGjkMaxIter) {
            // #if defined(DEBUG) || defined (_DEBUG)
            if (BulletGlobals.DEBUG) {
                System.err.printf("btGjkPairDetector maxIter exceeded:%i\n", curIter);
                System.err.printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", cachedSeparatingAxis.x, cachedSeparatingAxis.y, cachedSeparatingAxis.z, squaredDistance, minkowskiA.getShapeType().ordinal(), minkowskiB.getShapeType().ordinal());
            }
            // #endif
            break;
        }
        boolean check = (!simplexSolver.fullSimplex());
        if (!check) {
            // do we need this backup_closest here ?
            simplexSolver.backup_closest(cachedSeparatingAxis);
            break;
        }
    }
    if (checkSimplex) {
        simplexSolver.compute_points(pointOnA, pointOnB);
        normalInB.sub(pointOnA, pointOnB);
        float lenSqr = cachedSeparatingAxis.lengthSquared();
        // valid normal
        if (lenSqr < 0.0001f) {
            degenerateSimplex = 5;
        }
        if (lenSqr > BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) {
            float rlen = 1f / (float) Math.sqrt(lenSqr);
            // normalize
            normalInB.scale(rlen);
            float s = (float) Math.sqrt(squaredDistance);
            assert (s > 0f);
            tmp.scale((marginA / s), cachedSeparatingAxis);
            pointOnA.sub(tmp);
            tmp.scale((marginB / s), cachedSeparatingAxis);
            pointOnB.add(tmp);
            distance = ((1f / rlen) - margin);
            isValid = true;
            lastUsedMethod = 1;
        } else {
            lastUsedMethod = 2;
        }
    }
    boolean catchDegeneratePenetrationCase = (catchDegeneracies != 0 && penetrationDepthSolver != null && degenerateSimplex != 0 && ((distance + margin) < 0.01f));
    // if (checkPenetration && !isValid)
    if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) {
        // if there is no way to handle penetrations, bail out
        if (penetrationDepthSolver != null) {
            // Penetration depth case.
            BulletStats.gNumDeepPenetrationChecks++;
            boolean isValid2 = penetrationDepthSolver.calcPenDepth(simplexSolver, minkowskiA, minkowskiB, localTransA, localTransB, cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
            if (isValid2) {
                tmpNormalInB.sub(tmpPointOnB, tmpPointOnA);
                float lenSqr = tmpNormalInB.lengthSquared();
                if (lenSqr > (BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
                    tmpNormalInB.scale(1f / (float) Math.sqrt(lenSqr));
                    tmp.sub(tmpPointOnA, tmpPointOnB);
                    float distance2 = -tmp.length();
                    // only replace valid penetrations when the result is deeper (check)
                    if (!isValid || (distance2 < distance)) {
                        distance = distance2;
                        pointOnA.set(tmpPointOnA);
                        pointOnB.set(tmpPointOnB);
                        normalInB.set(tmpNormalInB);
                        isValid = true;
                        lastUsedMethod = 3;
                    } else {
                    }
                } else {
                    // isValid = false;
                    lastUsedMethod = 4;
                }
            } else {
                lastUsedMethod = 5;
            }
        }
    }
    if (isValid) {
        // #ifdef __SPU__
        // //spu_printf("distance\n");
        // #endif //__CELLOS_LV2__
        tmp.add(pointOnB, positionOffset);
        output.addContactPoint(normalInB, tmp, distance, BulletGlobals.the.get().getContactBreakingThreshold());
    // printf("gjk add:%f",distance);
    }
}