Examples with ConvexShape spacegraph.space3d.phys.shape.ConvexShape used on opensource projects

Example 1 with ConvexShape

use of spacegraph.space3d.phys.shape.ConvexShape in project narchy by automenta.

the class ConvexConvexAlgorithm method calculateTimeOfImpact.

@Override
public float calculateTimeOfImpact(Collidable col0, Collidable col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    v3 tmp = new v3();
    Transform tmpTrans1 = new Transform();
    Transform tmpTrans2 = new Transform();
    // Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
    // Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
    // col0->m_worldTransform,
    float resultFraction = 1f;
    tmp.sub(col0.getInterpolationWorldTransform(tmpTrans1), col0.getWorldTransform(tmpTrans2));
    if (tmp.lengthSquared() < col0.getCcdSquareMotionThreshold()) {
        tmp.sub(col1.getInterpolationWorldTransform(tmpTrans1), col1.getWorldTransform(tmpTrans2));
        if (tmp.lengthSquared() < col1.getCcdSquareMotionThreshold()) {
            return resultFraction;
        }
    }
    if (disableCcd) {
        return 1f;
    }
    Transform tmpTrans3 = new Transform();
    Transform tmpTrans4 = new Transform();
    // An adhoc way of testing the Continuous Collision Detection algorithms
    // One object is approximated as a sphere, to simplify things
    // Starting in penetration should report no time of impact
    // For proper CCD, better accuracy and handling of 'allowed' penetration should be added
    // also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
    // Convex0 against sphere for Convex1
    {
        ConvexShape convex0 = (ConvexShape) col0.shape();
        // todo: allow non-zero sphere sizes, for better approximation
        SphereShape sphere1 = new SphereShape(col1.getCcdSweptSphereRadius());
        ConvexCast.CastResult result = new ConvexCast.CastResult();
        voronoiSimplex.reset();
        // SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
        // /Simplification, one object is simplified as a sphere
        GjkConvexCast ccd1 = new GjkConvexCast(convex0, sphere1, voronoiSimplex);
        // ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
        if (ccd1.calcTimeOfImpact(col0.getWorldTransform(tmpTrans1), col0.getInterpolationWorldTransform(tmpTrans2), col1.getWorldTransform(tmpTrans3), col1.getInterpolationWorldTransform(tmpTrans4), result)) {
            if (col0.getHitFraction() > result.fraction) {
                col0.setHitFraction(result.fraction);
            }
            if (col1.getHitFraction() > result.fraction) {
                col1.setHitFraction(result.fraction);
            }
            if (resultFraction > result.fraction) {
                resultFraction = result.fraction;
            }
        }
    }
    // Sphere (for convex0) against Convex1
    ConvexShape convex1 = (ConvexShape) col1.shape();
    // todo: allow non-zero sphere sizes, for better approximation
    SphereShape sphere0 = new SphereShape(col0.getCcdSweptSphereRadius());
    ConvexCast.CastResult result = new ConvexCast.CastResult();
    VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
    // SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
    // /Simplification, one object is simplified as a sphere
    GjkConvexCast ccd1 = new GjkConvexCast(sphere0, convex1, voronoiSimplex);
    // ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
    if (ccd1.calcTimeOfImpact(col0.getWorldTransform(tmpTrans1), col0.getInterpolationWorldTransform(tmpTrans2), col1.getWorldTransform(tmpTrans3), col1.getInterpolationWorldTransform(tmpTrans4), result)) {
        if (col0.getHitFraction() > result.fraction) {
            col0.setHitFraction(result.fraction);
        }
        if (col1.getHitFraction() > result.fraction) {
            col1.setHitFraction(result.fraction);
        }
        if (resultFraction > result.fraction) {
            resultFraction = result.fraction;
        }
    }
    return resultFraction;
}

Example 2 with ConvexShape

use of spacegraph.space3d.phys.shape.ConvexShape in project narchy by automenta.

the class ConvexPlaneCollisionAlgorithm method processCollision.

@Override
public void processCollision(Collidable body0, Collidable body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    if (manifoldPtr == null) {
        return;
    }
    Transform tmpTrans = new Transform();
    Collidable convexObj = isSwapped ? body1 : body0;
    Collidable planeObj = isSwapped ? body0 : body1;
    ConvexShape convexShape = (ConvexShape) convexObj.shape();
    StaticPlaneShape planeShape = (StaticPlaneShape) planeObj.shape();
    boolean hasCollision = false;
    v3 planeNormal = planeShape.getPlaneNormal(new v3());
    float planeConstant = planeShape.getPlaneConstant();
    Transform planeInConvex = new Transform();
    convexObj.getWorldTransform(planeInConvex);
    planeInConvex.inverse();
    planeInConvex.mul(planeObj.getWorldTransform(tmpTrans));
    Transform convexInPlaneTrans = new Transform();
    convexInPlaneTrans.inverse(planeObj.getWorldTransform(tmpTrans));
    convexInPlaneTrans.mul(convexObj.getWorldTransform(tmpTrans));
    v3 tmp = new v3();
    tmp.negate(planeNormal);
    planeInConvex.basis.transform(tmp);
    v3 vtx = convexShape.localGetSupportingVertex(tmp, new v3());
    v3 vtxInPlane = new v3(vtx);
    convexInPlaneTrans.transform(vtxInPlane);
    float distance = (planeNormal.dot(vtxInPlane) - planeConstant);
    v3 vtxInPlaneProjected = new v3();
    tmp.scale(distance, planeNormal);
    vtxInPlaneProjected.sub(vtxInPlane, tmp);
    v3 vtxInPlaneWorld = new v3(vtxInPlaneProjected);
    planeObj.getWorldTransform(tmpTrans).transform(vtxInPlaneWorld);
    float breakingThresh = manifoldPtr.getContactBreakingThreshold();
    hasCollision = distance < breakingThresh;
    resultOut.setPersistentManifold(manifoldPtr);
    if (hasCollision) {
        // report a contact. internally this will be kept persistent, and contact reduction is done
        v3 normalOnSurfaceB = new v3(planeNormal);
        planeObj.getWorldTransform(tmpTrans).basis.transform(normalOnSurfaceB);
        v3 pOnB = new v3(vtxInPlaneWorld);
        resultOut.addContactPoint(normalOnSurfaceB, pOnB, distance, breakingThresh);
    }
    if (ownManifold) {
        if (manifoldPtr.numContacts() != 0) {
            resultOut.refreshContactPoints();
        }
    }
}

Example 3 with ConvexShape

use of spacegraph.space3d.phys.shape.ConvexShape in project narchy by automenta.

the class ConvexConvexAlgorithm method processCollision.

/**
 * Convex-Convex collision algorithm.
 */
@Override
public void processCollision(Collidable body0, Collidable body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    if (manifoldPtr == null) {
        // swapped?
        manifoldPtr = intersecter.getNewManifold(body0, body1);
        ownManifold = true;
    }
    resultOut.setPersistentManifold(manifoldPtr);
    // #ifdef USE_BT_GJKEPA
    // btConvexShape*				shape0(static_cast<btConvexShape*>(body0->getCollisionShape()));
    // btConvexShape*				shape1(static_cast<btConvexShape*>(body1->getCollisionShape()));
    // const btScalar				radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/);
    // btGjkEpaSolver::sResults	results;
    // if(btGjkEpaSolver::Collide(	shape0,body0->getWorldTransform(),
    // shape1,body1->getWorldTransform(),
    // radialmargin,results))
    // {
    // dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
    // resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
    // }
    // #else
    ConvexShape min0 = (ConvexShape) body0.shape();
    ConvexShape min1 = (ConvexShape) body1.shape();
    DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
    input.init();
    // JAVA NOTE: original: TODO: if (dispatchInfo.m_useContinuous)
    gjkPairDetector.setMinkowskiA(min0);
    gjkPairDetector.setMinkowskiB(min1);
    input.maximumDistanceSquared = min0.getMargin() + min1.getMargin() + manifoldPtr.getContactBreakingThreshold();
    input.maximumDistanceSquared *= input.maximumDistanceSquared;
    // input.m_stackAlloc = dispatchInfo.m_stackAllocator;
    // input.m_maximumDistanceSquared = btScalar(1e30);
    body0.getWorldTransform(input.transformA);
    body1.getWorldTransform(input.transformB);
    gjkPairDetector.getClosestPoints(input, resultOut);
    if (ownManifold) {
        resultOut.refreshContactPoints();
    }
}