Examples with Matrix3f spacegraph.util.math.Matrix3f used on opensource projects

Example 1 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class ContactConstraint method resolveSingleCollisionCombined.

/**
 * velocity + friction<br>
 * response between two dynamic objects with friction
 */
public static float resolveSingleCollisionCombined(Body3D body1, Body3D body2, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) {
    v3 tmpVec = new v3();
    v3 pos1 = contactPoint.getPositionWorldOnA(new v3());
    v3 pos2 = contactPoint.getPositionWorldOnB(new v3());
    v3 normal = contactPoint.normalWorldOnB;
    v3 rel_pos1 = new v3();
    rel_pos1.sub(pos1, body1.getCenterOfMassPosition(tmpVec));
    v3 rel_pos2 = new v3();
    rel_pos2.sub(pos2, body2.getCenterOfMassPosition(tmpVec));
    v3 vel1 = body1.getVelocityInLocalPoint(rel_pos1, new v3());
    v3 vel2 = body2.getVelocityInLocalPoint(rel_pos2, new v3());
    v3 vel = new v3();
    vel.sub(vel1, vel2);
    float rel_vel;
    rel_vel = normal.dot(vel);
    float Kfps = 1f / solverInfo.timeStep;
    // btScalar damping = solverInfo.m_damping ;
    float Kerp = solverInfo.erp;
    float Kcor = Kerp * Kfps;
    ConstraintPersistentData cpd = (ConstraintPersistentData) contactPoint.userPersistentData;
    assert (cpd != null);
    float distance = cpd.penetration;
    float positionalError = Kcor * -distance;
    // * damping;
    float velocityError = cpd.restitution - rel_vel;
    float penetrationImpulse = positionalError * cpd.jacDiagABInv;
    float velocityImpulse = velocityError * cpd.jacDiagABInv;
    float normalImpulse = penetrationImpulse + velocityImpulse;
    // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
    float oldNormalImpulse = cpd.appliedImpulse;
    float sum = oldNormalImpulse + normalImpulse;
    cpd.appliedImpulse = 0f > sum ? 0f : sum;
    normalImpulse = cpd.appliedImpulse - oldNormalImpulse;
    // #ifdef USE_INTERNAL_APPLY_IMPULSE
    v3 tmp = new v3();
    if (body1.getInvMass() != 0f) {
        tmp.scale(body1.getInvMass(), contactPoint.normalWorldOnB);
        body1.internalApplyImpulse(tmp, cpd.angularComponentA, normalImpulse);
    }
    if (body2.getInvMass() != 0f) {
        tmp.scale(body2.getInvMass(), contactPoint.normalWorldOnB);
        body2.internalApplyImpulse(tmp, cpd.angularComponentB, -normalImpulse);
    }
    // #else //USE_INTERNAL_APPLY_IMPULSE
    // body1.applyImpulse(normal*(normalImpulse), rel_pos1);
    // body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
    // #endif //USE_INTERNAL_APPLY_IMPULSE
    // friction
    body1.getVelocityInLocalPoint(rel_pos1, vel1);
    body2.getVelocityInLocalPoint(rel_pos2, vel2);
    vel.sub(vel1, vel2);
    rel_vel = normal.dot(vel);
    tmp.scale(rel_vel, normal);
    v3 lat_vel = new v3();
    lat_vel.sub(vel, tmp);
    float lat_rel_vel = lat_vel.length();
    float combinedFriction = cpd.friction;
    if (cpd.appliedImpulse > 0) {
        if (lat_rel_vel > BulletGlobals.FLT_EPSILON) {
            lat_vel.scale(1f / lat_rel_vel);
            v3 temp1 = new v3();
            temp1.cross(rel_pos1, lat_vel);
            body1.getInvInertiaTensorWorld(new Matrix3f()).transform(temp1);
            v3 temp2 = new v3();
            temp2.cross(rel_pos2, lat_vel);
            body2.getInvInertiaTensorWorld(new Matrix3f()).transform(temp2);
            v3 java_tmp1 = new v3();
            java_tmp1.cross(temp1, rel_pos1);
            v3 java_tmp2 = new v3();
            java_tmp2.cross(temp2, rel_pos2);
            tmp.add(java_tmp1, java_tmp2);
            float friction_impulse = lat_rel_vel / (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(tmp));
            float normal_impulse = cpd.appliedImpulse * combinedFriction;
            friction_impulse = Math.min(friction_impulse, normal_impulse);
            friction_impulse = Math.max(friction_impulse, -normal_impulse);
            tmp.scale(-friction_impulse, lat_vel);
            body1.impulse(tmp, rel_pos1);
            tmp.scale(friction_impulse, lat_vel);
            body2.impulse(tmp, rel_pos2);
        }
    }
    return normalImpulse;
}

Example 2 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class HingeConstraint method buildJacobian.

@Override
public void buildJacobian() {
    v3 tmp = new v3();
    v3 tmp1 = new v3();
    v3 tmp2 = new v3();
    v3 tmpVec = new v3();
    Matrix3f mat1 = new Matrix3f();
    Matrix3f mat2 = new Matrix3f();
    Transform centerOfMassA = rbA.getCenterOfMassTransform(new Transform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(new Transform());
    appliedImpulse = 0f;
    if (!angularOnly) {
        v3 pivotAInW = new v3(rbAFrame);
        centerOfMassA.transform(pivotAInW);
        v3 pivotBInW = new v3(rbBFrame);
        centerOfMassB.transform(pivotBInW);
        v3 relPos = new v3();
        relPos.sub(pivotBInW, pivotAInW);
        v3[] normal = /*[3]*/
        { new v3(), new v3(), new v3() };
        if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) {
            normal[0].set(relPos);
            normal[0].normalize();
        } else {
            normal[0].set(1f, 0f, 0f);
        }
        TransformUtil.planeSpace1(normal[0], normal[1], normal[2]);
        for (int i = 0; i < 3; i++) {
            mat1.transpose(centerOfMassA.basis);
            mat2.transpose(centerOfMassB.basis);
            tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
            tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
            jac[i].init(mat1, mat2, tmp1, tmp2, normal[i], rbA.getInvInertiaDiagLocal(new v3()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(new v3()), rbB.getInvMass());
        }
    }
    // calculate two perpendicular jointAxis, orthogonal to hingeAxis
    // these two jointAxis require equal angular velocities for both bodies
    // this is unused for now, it's a todo
    v3 jointAxis0local = new v3();
    v3 jointAxis1local = new v3();
    rbAFrame.basis.getColumn(2, tmp);
    TransformUtil.planeSpace1(tmp, jointAxis0local, jointAxis1local);
    // TODO: check this
    // getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
    v3 jointAxis0 = new v3(jointAxis0local);
    centerOfMassA.basis.transform(jointAxis0);
    v3 jointAxis1 = new v3(jointAxis1local);
    centerOfMassA.basis.transform(jointAxis1);
    v3 hingeAxisWorld = new v3();
    rbAFrame.basis.getColumn(2, hingeAxisWorld);
    centerOfMassA.basis.transform(hingeAxisWorld);
    mat1.transpose(centerOfMassA.basis);
    mat2.transpose(centerOfMassB.basis);
    jacAng[0].init(jointAxis0, mat1, mat2, rbA.getInvInertiaDiagLocal(new v3()), rbB.getInvInertiaDiagLocal(new v3()));
    // JAVA NOTE: reused mat1 and mat2, as recomputation is not needed
    jacAng[1].init(jointAxis1, mat1, mat2, rbA.getInvInertiaDiagLocal(new v3()), rbB.getInvInertiaDiagLocal(new v3()));
    // JAVA NOTE: reused mat1 and mat2, as recomputation is not needed
    jacAng[2].init(hingeAxisWorld, mat1, mat2, rbA.getInvInertiaDiagLocal(new v3()), rbB.getInvInertiaDiagLocal(new v3()));
    // Compute limit information
    float hingeAngle = getHingeAngle();
    // set bias, sign, clear accumulator
    correction = 0f;
    limitSign = 0f;
    solveLimit = false;
    accLimitImpulse = 0f;
    if (lowerLimit < upperLimit) {
        if (hingeAngle <= lowerLimit * limitSoftness) {
            correction = (lowerLimit - hingeAngle);
            limitSign = 1.0f;
            solveLimit = true;
        } else if (hingeAngle >= upperLimit * limitSoftness) {
            correction = upperLimit - hingeAngle;
            limitSign = -1.0f;
            solveLimit = true;
        }
    }
    // Compute K = J*W*J' for hinge axis
    v3 axisA = new v3();
    rbAFrame.basis.getColumn(2, axisA);
    centerOfMassA.basis.transform(axisA);
    kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + getRigidBodyB().computeAngularImpulseDenominator(axisA));
}

Example 3 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class AabbUtil2 method transformAabb.

public static void transformAabb(v3 localAabbMin, v3 localAabbMax, float margin, Transform trans, v3 aabbMinOut, v3 aabbMaxOut) {
    assert (localAabbMin.x <= localAabbMax.x);
    assert (localAabbMin.y <= localAabbMax.y);
    assert (localAabbMin.z <= localAabbMax.z);
    v3 localHalfExtents = new v3();
    localHalfExtents.sub(localAabbMax, localAabbMin);
    localHalfExtents.scale(0.5f);
    localHalfExtents.x += margin;
    localHalfExtents.y += margin;
    localHalfExtents.z += margin;
    v3 localCenter = new v3();
    localCenter.add(localAabbMax, localAabbMin);
    localCenter.scale(0.5f);
    Matrix3f abs_b = new Matrix3f(trans.basis);
    MatrixUtil.absolute(abs_b);
    v3 center = new v3(localCenter);
    trans.transform(center);
    v3 extent = new v3();
    v3 tmp = new v3();
    abs_b.getRow(0, tmp);
    extent.x = tmp.dot(localHalfExtents);
    abs_b.getRow(1, tmp);
    extent.y = tmp.dot(localHalfExtents);
    abs_b.getRow(2, tmp);
    extent.z = tmp.dot(localHalfExtents);
    aabbMinOut.sub(center, extent);
    aabbMaxOut.add(center, extent);
}

Example 4 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class Body3D method updateInertiaTensor.

public void updateInertiaTensor() {
    Matrix3f mat1 = new Matrix3f();
    MatrixUtil.scale(mat1, transform.basis, invInertiaLocal);
    Matrix3f mat2 = new Matrix3f(transform.basis);
    mat2.transpose();
    invInertiaTensorWorld.mul(mat1, mat2);
}

Example 5 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class Collisions 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, Collidable collidable, CollisionShape collisionShape, Transform colObjWorldTransform, ConvexResultCallback resultCallback, float allowedPenetration) {
    if (collisionShape.isConvex()) {
        ConvexCast.CastResult castResult = new ConvexCast.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(collidable, 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 = new Transform();
                worldTocollisionObject.inverse(colObjWorldTransform);
                v3 convexFromLocal = new v3();
                convexFromLocal.set(convexFromTrans);
                worldTocollisionObject.transform(convexFromLocal);
                v3 convexToLocal = new v3();
                convexToLocal.set(convexToTrans);
                worldTocollisionObject.transform(convexToLocal);
                // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
                Transform rotationXform = new Transform();
                Matrix3f tmpMat = new Matrix3f();
                tmpMat.mul(worldTocollisionObject.basis, convexToTrans.basis);
                rotationXform.set(tmpMat);
                BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, convexFromTrans, convexToTrans, resultCallback, collidable, triangleMesh, colObjWorldTransform);
                tccb.hitFraction = resultCallback.closestHitFraction;
                tccb.normalInWorldSpace = true;
                v3 boxMinLocal = new v3();
                v3 boxMaxLocal = new v3();
                castShape.getAabb(rotationXform, boxMinLocal, boxMaxLocal);
                triangleMesh.performConvexcast(tccb, convexFromLocal, convexToLocal, boxMinLocal, boxMaxLocal);
            } else {
                ConcaveShape triangleMesh = (ConcaveShape) collisionShape;
                Transform worldTocollisionObject = new Transform();
                worldTocollisionObject.inverse(colObjWorldTransform);
                v3 convexFromLocal = new v3();
                convexFromLocal.set(convexFromTrans);
                worldTocollisionObject.transform(convexFromLocal);
                v3 convexToLocal = new v3();
                convexToLocal.set(convexToTrans);
                worldTocollisionObject.transform(convexToLocal);
                // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
                Transform rotationXform = new Transform();
                Matrix3f tmpMat = new Matrix3f();
                tmpMat.mul(worldTocollisionObject.basis, convexToTrans.basis);
                rotationXform.set(tmpMat);
                BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, convexFromTrans, convexToTrans, resultCallback, collidable, triangleMesh, colObjWorldTransform);
                tccb.hitFraction = resultCallback.closestHitFraction;
                tccb.normalInWorldSpace = false;
                v3 boxMinLocal = new v3();
                v3 boxMaxLocal = new v3();
                castShape.getAabb(rotationXform, boxMinLocal, boxMaxLocal);
                v3 rayAabbMinLocal = new v3(convexFromLocal);
                VectorUtil.setMin(rayAabbMinLocal, convexToLocal);
                v3 rayAabbMaxLocal = new v3(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, new Transform());
                    CollisionShape childCollisionShape = compoundShape.getChildShape(i);
                    Transform childWorldTrans = new Transform();
                    childWorldTrans.mul(colObjWorldTransform, childTrans);
                    // replace collision shape so that callback can determine the triangle
                    CollisionShape saveCollisionShape = collidable.shape();
                    collidable.internalSetTemporaryCollisionShape(childCollisionShape);
                    objectQuerySingle(castShape, convexFromTrans, convexToTrans, collidable, childCollisionShape, childWorldTrans, resultCallback, allowedPenetration);
                    // restore
                    collidable.internalSetTemporaryCollisionShape(saveCollisionShape);
                }
            }
        }
    }
}