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

Example 51 with Transform

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

the class Collisions method rayTest.

/**
 * rayTest performs a raycast on all objects in the CollisionWorld, and calls the resultCallback.
 * This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
 */
public RayResultCallback rayTest(v3 rayFromWorld, v3 rayToWorld, RayResultCallback resultCallback, VoronoiSimplexSolver simplexSolver) {
    Transform rayFromTrans = new Transform(rayFromWorld);
    Transform rayToTrans = new Transform(rayToWorld);
    // go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
    v3 collisionObjectAabbMin = v(), collisionObjectAabbMax = v();
    float[] hitLambda = new float[1];
    // Transform tmpTrans = new Transform();
    List<Collidable> objs = collidables();
    for (int i = 0, objsSize = objs.size(); i < objsSize; i++) {
        Collidable collidable = objs.get(i);
        // terminate further ray tests, once the closestHitFraction reached zero
        if (resultCallback.closestHitFraction == 0f) {
            break;
        }
        // return array[index];
        if (collidable != null) {
            Broadphasing broadphaseHandle = collidable.broadphase;
            // only perform raycast if filterMask matches
            if (broadphaseHandle != null && resultCallback.needsCollision(broadphaseHandle)) {
                // RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
                CollisionShape shape = collidable.shape();
                Transform worldTransform = collidable.transform;
                shape.getAabb(worldTransform, collisionObjectAabbMin, collisionObjectAabbMax);
                if (!collisionObjectAabbMin.isFinite() || !collisionObjectAabbMax.isFinite())
                    continue;
                hitLambda[0] = resultCallback.closestHitFraction;
                if (AabbUtil2.rayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda)) {
                    simplexSolver.reset();
                    rayTestSingle(rayFromTrans, rayToTrans, collidable, shape, worldTransform, simplexSolver, resultCallback);
                }
            }
        }
    }
    return resultCallback;
}

Example 52 with Transform

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

the class PolyhedralConvexShape method calculateLocalInertia.

@Override
public void calculateLocalInertia(float mass, v3 inertia) {
    // not yet, return box inertia
    float margin = getMargin();
    Transform ident = new Transform();
    ident.setIdentity();
    v3 aabbMin = new v3(), aabbMax = new v3();
    getAabb(ident, aabbMin, aabbMax);
    v3 halfExtents = new v3();
    halfExtents.sub(aabbMax, aabbMin);
    halfExtents.scale(0.5f);
    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);
}

Example 53 with Transform

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

the class TriangleMeshShape method localGetSupportingVertex.

public v3 localGetSupportingVertex(v3 vec, v3 out) {
    v3 tmp = new v3();
    v3 supportVertex = out;
    Transform ident = new Transform();
    ident.setIdentity();
    SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident);
    v3 aabbMax = new v3();
    aabbMax.set(1e30f, 1e30f, 1e30f);
    tmp.negate(aabbMax);
    processAllTriangles(supportCallback, tmp, aabbMax);
    supportCallback.getSupportVertexLocal(supportVertex);
    return out;
}

Example 54 with Transform

use of spacegraph.space3d.phys.math.Transform 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 55 with Transform

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

the class CompoundShape method calculateLocalInertia.

@Override
public void calculateLocalInertia(float mass, v3 inertia) {
    // approximation: take the inertia from the aabb for now
    Transform ident = new Transform();
    ident.setIdentity();
    v3 aabbMin = new v3(), aabbMax = new v3();
    getAabb(ident, aabbMin, aabbMax);
    v3 halfExtents = new v3();
    halfExtents.sub(aabbMax, aabbMin);
    halfExtents.scale(0.5f);
    float lx = 2f * halfExtents.x;
    float ly = 2f * halfExtents.y;
    float lz = 2f * halfExtents.z;
    inertia.x = (mass / 12f) * (ly * ly + lz * lz);
    inertia.y = (mass / 12f) * (lx * lx + lz * lz);
    inertia.z = (mass / 12f) * (lx * lx + ly * ly);
}