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

Example 21 with Matrix3f

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

the class Dynamics3D method debugDrawObject.

public static void debugDrawObject(IDebugDraw debugDrawer, Transform worldTransform, CollisionShape shape, v3 color) {
    v3 tmp = new v3();
    v3 tmp2 = new v3();
    // Draw a small simplex at the center of the object
    v3 start = new v3(worldTransform);
    tmp.set(1f, 0f, 0f);
    Matrix3f transformBasis = worldTransform.basis;
    transformBasis.transform(tmp);
    tmp.add(start);
    tmp2.set(1f, 0f, 0f);
    debugDrawer.drawLine(start, tmp, tmp2);
    tmp.set(0f, 1f, 0f);
    transformBasis.transform(tmp);
    tmp.add(start);
    tmp2.set(0f, 1f, 0f);
    debugDrawer.drawLine(start, tmp, tmp2);
    tmp.set(0f, 0f, 1f);
    transformBasis.transform(tmp);
    tmp.add(start);
    tmp2.set(0f, 0f, 1f);
    debugDrawer.drawLine(start, tmp, tmp2);
// JAVA TODO: debugDrawObject, note that this commented code is from old version, use actual version when implementing
// if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
// {
// const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
// for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
// {
// btTransform childTrans = compoundShape->getChildTransform(i);
// const btCollisionShape* colShape = compoundShape->getChildShape(i);
// debugDrawObject(worldTransform*childTrans,colShape,color);
// }
// 
// } else
// {
// switch (shape->getShapeType())
// {
// 
// case SPHERE_SHAPE_PROXYTYPE:
// {
// const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
// btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
// 
// debugDrawSphere(radius, worldTransform, color);
// break;
// }
// case MULTI_SPHERE_SHAPE_PROXYTYPE:
// {
// const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
// 
// for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
// {
// btTransform childTransform = worldTransform;
// childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
// debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
// }
// 
// break;
// }
// case CAPSULE_SHAPE_PROXYTYPE:
// {
// const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
// 
// btScalar radius = capsuleShape->getRadius();
// btScalar halfHeight = capsuleShape->getHalfHeight();
// 
// // Draw the ends
// {
// btTransform childTransform = worldTransform;
// childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0);
// debugDrawSphere(radius, childTransform, color);
// }
// 
// {
// btTransform childTransform = worldTransform;
// childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0);
// debugDrawSphere(radius, childTransform, color);
// }
// 
// // Draw some additional lines
// btVector3 start = worldTransform.getOrigin();
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), color);
// 
// break;
// }
// case CONE_SHAPE_PROXYTYPE:
// {
// const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
// btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
// btScalar height = coneShape->getHeight();//+coneShape->getMargin();
// btVector3 start = worldTransform.getOrigin();
// 
// int upAxis= coneShape->getConeUpIndex();
// 
// 
// btVector3	offsetHeight(0,0,0);
// offsetHeight[upAxis] = height * btScalar(0.5);
// btVector3	offsetRadius(0,0,0);
// offsetRadius[(upAxis+1)%3] = radius;
// btVector3	offset2Radius(0,0,0);
// offset2Radius[(upAxis+2)%3] = radius;
// 
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
// 
// 
// 
// break;
// 
// }
// case CYLINDER_SHAPE_PROXYTYPE:
// {
// const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
// int upAxis = cylinder->getUpAxis();
// btScalar radius = cylinder->getRadius();
// btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
// btVector3 start = worldTransform.getOrigin();
// btVector3	offsetHeight(0,0,0);
// offsetHeight[upAxis] = halfHeight;
// btVector3	offsetRadius(0,0,0);
// offsetRadius[(upAxis+1)%3] = radius;
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
// break;
// }
// default:
// {
// 
// if (shape->isConcave())
// {
// btConcaveShape* concaveMesh = (btConcaveShape*) shape;
// 
// //todo pass camera, for some culling
// btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
// btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
// 
// DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
// concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
// 
// }
// 
// if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
// {
// btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
// //todo: pass camera for some culling
// btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
// btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
// //DebugDrawcallback drawCallback;
// DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
// convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
// }
// 
// 
// /// for polyhedral shapes
// if (shape->isPolyhedral())
// {
// btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
// 
// int i;
// for (i=0;i<polyshape->getNumEdges();i++)
// {
// btPoint3 a,b;
// polyshape->getEdge(i,a,b);
// btVector3 wa = worldTransform * a;
// btVector3 wb = worldTransform * b;
// getDebugDrawer()->drawLine(wa,wb,color);
// 
// }
// 
// 
// }
// }
// }
// }
}

Example 22 with Matrix3f

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

the class HeightfieldTerrainShape method getAabb.

@Override
public void getAabb(Transform trans, v3 aabbMin, v3 aabbMax) {
    v3 tmp = new v3();
    v3 localHalfExtents = new v3();
    localHalfExtents.sub(m_localAabbMax, m_localAabbMin);
    VectorUtil.mul(localHalfExtents, localHalfExtents, m_localScaling);
    // localHalfExtents.mul(localHalfExtents,m_localScaling);
    localHalfExtents.scale(0.5f);
    v3 localOrigin = new v3();
    localOrigin.set(0f, 0f, 0f);
    VectorUtil.setCoord(localOrigin, m_upAxis, (m_minHeight + m_maxHeight) * 0.5f);
    VectorUtil.mul(localOrigin, localOrigin, m_localScaling);
    Matrix3f abs_b = new Matrix3f(trans.basis);
    MatrixUtil.absolute(abs_b);
    v3 center = new v3(trans);
    v3 extent = 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);
    v3 margin = new v3();
    margin.set(getMargin(), getMargin(), getMargin());
    extent.add(margin);
    aabbMin.sub(center, extent);
    aabbMax.add(center, extent);
}

Example 23 with Matrix3f

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

the class SequentialImpulseConstrainer method addFrictionConstraint.

protected void addFrictionConstraint(v3 normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, ManifoldPoint cp, v3 rel_pos1, v3 rel_pos2, Collidable colObj0, Collidable colObj1, float relaxation) {
    Body3D body0 = Body3D.ifDynamic(colObj0);
    Body3D body1 = Body3D.ifDynamic(colObj1);
    SolverConstraint solverConstraint = new SolverConstraint();
    tmpSolverFrictionConstraintPool.add(solverConstraint);
    solverConstraint.contactNormal.set(normalAxis);
    solverConstraint.solverBodyIdA = solverBodyIdA;
    solverConstraint.solverBodyIdB = solverBodyIdB;
    solverConstraint.constraintType = SolverConstraint.SolverConstraintType.SOLVER_FRICTION_1D;
    solverConstraint.frictionIndex = frictionIndex;
    solverConstraint.friction = cp.combinedFriction;
    solverConstraint.originalContactPoint = null;
    solverConstraint.appliedImpulse = 0f;
    solverConstraint.appliedPushImpulse = 0f;
    solverConstraint.penetration = 0f;
    v3 ftorqueAxis1 = new v3();
    Matrix3f tmpMat = new Matrix3f();
    ftorqueAxis1.cross(rel_pos1, solverConstraint.contactNormal);
    solverConstraint.relpos1CrossNormal.set(ftorqueAxis1);
    if (body0 != null) {
        solverConstraint.angularComponentA.set(ftorqueAxis1);
        body0.getInvInertiaTensorWorld(tmpMat).transform(solverConstraint.angularComponentA);
    } else {
        solverConstraint.angularComponentA.set(0f, 0f, 0f);
    }
    ftorqueAxis1.cross(rel_pos2, solverConstraint.contactNormal);
    solverConstraint.relpos2CrossNormal.set(ftorqueAxis1);
    if (body1 != null) {
        solverConstraint.angularComponentB.set(ftorqueAxis1);
        body1.getInvInertiaTensorWorld(tmpMat).transform(solverConstraint.angularComponentB);
    } else {
        solverConstraint.angularComponentB.set(0f, 0f, 0f);
    }
    // #ifdef COMPUTE_IMPULSE_DENOM
    // btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
    // btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
    // #else
    v3 vec = new v3();
    float denom0 = 0f;
    float denom1 = 0f;
    if (body0 != null) {
        vec.cross(solverConstraint.angularComponentA, rel_pos1);
        denom0 = body0.getInvMass() + normalAxis.dot(vec);
    }
    if (body1 != null) {
        vec.cross(solverConstraint.angularComponentB, rel_pos2);
        denom1 = body1.getInvMass() + normalAxis.dot(vec);
    }
    // #endif //COMPUTE_IMPULSE_DENOM
    float denom = relaxation / (denom0 + denom1);
    solverConstraint.jacDiagABInv = denom;
}

Example 24 with Matrix3f

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

the class SequentialImpulseConstrainer method prepareConstraints.

protected void prepareConstraints(PersistentManifold manifoldPtr, ContactSolverInfo info) {
    Body3D body0 = (Body3D) manifoldPtr.getBody0();
    Body3D body1 = (Body3D) manifoldPtr.getBody1();
    // only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
    // #ifdef FORCE_REFESH_CONTACT_MANIFOLDS
    // manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
    // #endif //FORCE_REFESH_CONTACT_MANIFOLDS
    int numpoints = manifoldPtr.numContacts();
    BulletStats.gTotalContactPoints += numpoints;
    v3 tmpVec = new v3();
    v3 pos1 = new v3();
    v3 pos2 = new v3();
    v3 rel_pos1 = new v3();
    v3 rel_pos2 = new v3();
    v3 vel1 = new v3();
    v3 vel2 = new v3();
    v3 vel = new v3();
    v3 totalImpulse = new v3();
    v3 torqueAxis0 = new v3();
    v3 torqueAxis1 = new v3();
    v3 ftorqueAxis0 = new v3();
    v3 ftorqueAxis1 = new v3();
    final Transform tt1 = new Transform(), tt2 = new Transform();
    JacobianEntry jac = new JacobianEntry();
    for (int i = 0; i < numpoints; i++) {
        ManifoldPoint cp = manifoldPtr.getContactPoint(i);
        if (cp.distance1 <= 0f) {
            cp.getPositionWorldOnA(pos1);
            cp.getPositionWorldOnB(pos2);
            rel_pos1.sub(pos1, body0.transform);
            rel_pos2.sub(pos2, body1.transform);
            // this jacobian entry is re-used for all iterations
            Matrix3f mat1 = body0.transform.basis;
            mat1.transpose();
            Matrix3f mat2 = body1.transform.basis;
            mat2.transpose();
            jac.init(mat1, mat2, rel_pos1, rel_pos2, cp.normalWorldOnB, body0.invInertiaLocal, body0.getInvMass(), body1.invInertiaLocal, body1.getInvMass());
            float jacDiagAB = jac.Adiag;
            ConstraintPersistentData cpd = (ConstraintPersistentData) cp.userPersistentData;
            if (cpd != null) {
                // might be invalid
                cpd.persistentLifeTime++;
                if (cpd.persistentLifeTime != cp.lifeTime) {
                    // printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
                    // new (cpd) btConstraintPersistentData;
                    cpd.reset();
                    cpd.persistentLifeTime = cp.lifeTime;
                } else {
                // printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
                }
            } else {
                // todo: should this be in a pool?
                // void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
                // cpd = new (mem)btConstraintPersistentData;
                cpd = new ConstraintPersistentData();
                // assert(cpd != null);
                // totalCpd++;
                // printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
                cp.userPersistentData = cpd;
                cpd.persistentLifeTime = cp.lifeTime;
            // printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
            }
            assert (cpd != null);
            cpd.jacDiagABInv = 1f / jacDiagAB;
            // Dependent on Rigidbody A and B types, fetch the contact/friction response func
            // perhaps do a similar thing for friction/restutution combiner funcs...
            cpd.frictionSolverFunc = frictionDispatch[body0.frictionSolverType][body1.frictionSolverType];
            cpd.contactSolverFunc = contactDispatch[body0.contactSolverType][body1.contactSolverType];
            body0.getVelocityInLocalPoint(rel_pos1, vel1);
            body1.getVelocityInLocalPoint(rel_pos2, vel2);
            vel.sub(vel1, vel2);
            float rel_vel;
            rel_vel = cp.normalWorldOnB.dot(vel);
            float combinedRestitution = cp.combinedRestitution;
            // /btScalar(info.m_numIterations);
            cpd.penetration = cp.distance1;
            cpd.friction = cp.combinedFriction;
            cpd.restitution = restitutionCurve(rel_vel, combinedRestitution);
            if (cpd.restitution <= 0f) {
                cpd.restitution = 0f;
            }
            // restitution and penetration work in same direction so
            // rel_vel
            float penVel = -cpd.penetration / info.timeStep;
            if (cpd.restitution > penVel) {
                cpd.penetration = 0f;
            }
            float relaxation = info.damping;
            if ((info.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) {
                cpd.appliedImpulse *= relaxation;
            } else {
                cpd.appliedImpulse = 0f;
            }
            // for friction
            cpd.prevAppliedImpulse = cpd.appliedImpulse;
            // re-calculate friction direction every frame, todo: check if this is really needed
            TransformUtil.planeSpace1(cp.normalWorldOnB, cpd.frictionWorldTangential0, cpd.frictionWorldTangential1);
            // #define NO_FRICTION_WARMSTART 1
            // #ifdef NO_FRICTION_WARMSTART
            cpd.accumulatedTangentImpulse0 = 0f;
            cpd.accumulatedTangentImpulse1 = 0f;
            // #endif //NO_FRICTION_WARMSTART
            float denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential0);
            float denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential0);
            float denom = relaxation / (denom0 + denom1);
            cpd.jacDiagABInvTangent0 = denom;
            denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential1);
            denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential1);
            denom = relaxation / (denom0 + denom1);
            cpd.jacDiagABInvTangent1 = denom;
            // btVector3 totalImpulse =
            // //#ifndef NO_FRICTION_WARMSTART
            // //cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
            // //cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
            // //#endif //NO_FRICTION_WARMSTART
            // cp.normalWorldOnB*cpd.appliedImpulse;
            totalImpulse.scale(cpd.appliedImpulse, cp.normalWorldOnB);
            // /
            torqueAxis0.cross(rel_pos1, cp.normalWorldOnB);
            cpd.angularComponentA.set(torqueAxis0);
            body0.invInertiaTensorWorld.transform(cpd.angularComponentA);
            torqueAxis1.cross(rel_pos2, cp.normalWorldOnB);
            cpd.angularComponentB.set(torqueAxis1);
            body1.invInertiaTensorWorld.transform(cpd.angularComponentB);
            ftorqueAxis0.cross(rel_pos1, cpd.frictionWorldTangential0);
            cpd.frictionAngularComponent0A.set(ftorqueAxis0);
            body0.invInertiaTensorWorld.transform(cpd.frictionAngularComponent0A);
            ftorqueAxis1.cross(rel_pos1, cpd.frictionWorldTangential1);
            cpd.frictionAngularComponent1A.set(ftorqueAxis1);
            body0.invInertiaTensorWorld.transform(cpd.frictionAngularComponent1A);
            ftorqueAxis0.cross(rel_pos2, cpd.frictionWorldTangential0);
            cpd.frictionAngularComponent0B.set(ftorqueAxis0);
            body1.invInertiaTensorWorld.transform(cpd.frictionAngularComponent0B);
            ftorqueAxis1.cross(rel_pos2, cpd.frictionWorldTangential1);
            cpd.frictionAngularComponent1B.set(ftorqueAxis1);
            body1.invInertiaTensorWorld.transform(cpd.frictionAngularComponent1B);
            // /
            // apply previous frames impulse on both bodies
            body0.impulse(totalImpulse, rel_pos1);
            tmpVec.negate(totalImpulse);
            body1.impulse(tmpVec, rel_pos2);
        }
    }
}

Example 25 with Matrix3f

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

the class ScaledBvhTriangleMeshShape method getAabb.

@Override
public void getAabb(Transform trans, v3 aabbMin, v3 aabbMax) {
    v3 localAabbMin = bvhTriMeshShape.getLocalAabbMin(new v3());
    v3 localAabbMax = bvhTriMeshShape.getLocalAabbMax(new v3());
    v3 tmpLocalAabbMin = new v3();
    v3 tmpLocalAabbMax = new v3();
    VectorUtil.mul(tmpLocalAabbMin, localAabbMin, localScaling);
    VectorUtil.mul(tmpLocalAabbMax, localAabbMax, localScaling);
    localAabbMin.x = (localScaling.x >= 0f) ? tmpLocalAabbMin.x : tmpLocalAabbMax.x;
    localAabbMin.y = (localScaling.y >= 0f) ? tmpLocalAabbMin.y : tmpLocalAabbMax.y;
    localAabbMin.z = (localScaling.z >= 0f) ? tmpLocalAabbMin.z : tmpLocalAabbMax.z;
    localAabbMax.x = (localScaling.x <= 0f) ? tmpLocalAabbMin.x : tmpLocalAabbMax.x;
    localAabbMax.y = (localScaling.y <= 0f) ? tmpLocalAabbMin.y : tmpLocalAabbMax.y;
    localAabbMax.z = (localScaling.z <= 0f) ? tmpLocalAabbMin.z : tmpLocalAabbMax.z;
    v3 localHalfExtents = new v3();
    localHalfExtents.sub(localAabbMax, localAabbMin);
    localHalfExtents.scale(0.5f);
    float margin = bvhTriMeshShape.getMargin();
    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);
    aabbMin.sub(center, extent);
    aabbMax.add(center, extent);
}