Examples with Body3D spacegraph.space3d.phys.Body3D used on opensource projects

Example 11 with Body3D

use of spacegraph.space3d.phys.Body3D in project narchy by automenta.

the class OrbMouse method mouseGrabOn.

private ClosestRay mouseGrabOn() {
    if (pickConstraint == null && pickedBody != null) {
        pickedBody.setActivationState(Collidable.DISABLE_DEACTIVATION);
        Body3D body = pickedBody;
        v3 pickPos = new v3(rayCallback.hitPointWorld);
        Transform tmpTrans = body.transform;
        tmpTrans.inverse();
        v3 localPivot = new v3(pickPos);
        tmpTrans.transform(localPivot);
        Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
        p2p.impulseClamp = 3f;
        // save mouse position for dragging
        gOldPickingPos.set(rayCallback.rayToWorld);
        v3 eyePos = new v3(space.camPos);
        v3 tmp = new v3();
        tmp.sub(pickPos, eyePos);
        gOldPickingDist = tmp.length();
        // very weak constraint for picking
        p2p.tau = 0.1f;
        space.dyn.addConstraint(p2p);
        pickConstraint = p2p;
    // body.setActivationState(Collidable.DISABLE_DEACTIVATION);
    // v3 pickPos = v(rayCallback.hitPointWorld);
    // 
    // Transform tmpTrans = body.getCenterOfMassTransform(new Transform());
    // tmpTrans.inverse();
    // v3 localPivot = v(pickPos);
    // tmpTrans.transform(localPivot);
    // // save mouse position for dragging
    // gOldPickingPos.set(rayCallback.rayToWorld);
    // v3 eyePos = v(space.camPos);
    // v3 tmp = v();
    // tmp.sub(pickPos, eyePos);
    // gOldPickingDist = tmp.length();
    // 
    // 
    // // other exclusions?
    // if (!(body.isStaticObject() || body.isKinematicObject())) {
    // pickedBody = body;
    // 
    // 
    // Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
    // space.dyn.addConstraint(p2p);
    // pickConstraint = p2p;
    // 
    // // very weak constraint for picking
    // p2p.tau = 0.02f;
    // } else {
    // if (directDrag == null) {
    // directDrag = body;
    // 
    // }
    // }
    }
    return rayCallback;
// }
}

Example 12 with Body3D

use of spacegraph.space3d.phys.Body3D in project narchy by automenta.

the class SimpleSpatial method create.

protected Body3D create(Dynamics3D world) {
    Body3D b = newBody(collidable());
    b.setData(this);
    return b;
}

Example 13 with Body3D

use of spacegraph.space3d.phys.Body3D 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 14 with Body3D

use of spacegraph.space3d.phys.Body3D 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 15 with Body3D

use of spacegraph.space3d.phys.Body3D in project narchy by automenta.

the class RagDoll method localCreateRigidBody.

// public void destroy(Dynamics w) {
// int i;
// 
// // Remove all constraints
// for (i = 0; i < JointType.JOINT_COUNT.ordinal(); ++i) {
// w.removeConstraint(joints[i]);
// //joints[i].destroy();
// joints[i] = null;
// }
// 
// // Remove all bodies and shapes
// for (i = 0; i < BodyPart.BODYPART_COUNT.ordinal(); ++i) {
// w.remove(bodies[i]);
// 
// bodies[i] = null;
// shapes[i] = null;
// }
// }
private static Body3D localCreateRigidBody(float mass, Transform startTransform, CollisionShape shape) {
    // stack.vectors.push();
    // try {
    Body3D body = Dynamics3D.newBody(mass, shape, startTransform, +1, -1);
    body.setCenterOfMassTransform(startTransform);
    return body;
// } finally {
// stack.vectors.pop();
// }
}