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

Example 1 with SphereShape

use of spacegraph.space3d.phys.shape.SphereShape 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 SphereShape

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

the class SphereSphereCollisionAlgorithm method processCollision.

@Override
public void processCollision(Collidable col0, Collidable col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    if (manifoldPtr == null) {
        return;
    }
    resultOut.setPersistentManifold(manifoldPtr);
    v3 diff = new v3();
    diff.sub(col0.transform, col1.transform);
    float lenSq = diff.lengthSquared();
    SphereShape sphere0 = (SphereShape) col0.shape();
    SphereShape sphere1 = (SphereShape) col1.shape();
    float radius0 = sphere0.getRadius();
    float radius1 = sphere1.getRadius();
    // #ifdef CLEAR_MANIFOLD
    // manifoldPtr.clearManifold(); // don't do this, it disables warmstarting
    // #endif
    // if distance positive, don't generate a new contact
    float r01 = radius0 + radius1;
    if (lenSq > r01 * r01) {
        // #ifndef CLEAR_MANIFOLD
        resultOut.refreshContactPoints();
        // #endif //CLEAR_MANIFOLD
        return;
    }
    v3 normalOnSurfaceB = new v3(1, 0, 0);
    float len = (float) Math.sqrt(lenSq);
    // distance (negative means penetration)
    float dist = len - r01;
    if (len > BulletGlobals.FLT_EPSILON) {
        normalOnSurfaceB.scale(1f / len, diff);
    }
    v3 tmp = new v3();
    // point on A (worldspace)
    v3 pos0 = new v3();
    tmp.scale(radius0, normalOnSurfaceB);
    pos0.sub(col0.transform, tmp);
    // point on B (worldspace)
    v3 pos1 = new v3();
    tmp.scale(radius1, normalOnSurfaceB);
    pos1.add(col1.transform, tmp);
    // report a contact. internally this will be kept persistent, and contact reduction is done
    resultOut.addContactPoint(normalOnSurfaceB, pos1, dist, manifoldPtr.getContactBreakingThreshold());
    // #ifndef CLEAR_MANIFOLD
    resultOut.refreshContactPoints();
// #endif //CLEAR_MANIFOLD
}

Example 3 with SphereShape

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

the class Dynamics3D method integrateTransforms.

protected void integrateTransforms(float timeStep) {
    v3 tmp = new v3();
    Transform predictedTrans = new Transform();
    SphereShape tmpSphere = new SphereShape(1);
    for (int i = 0, collidableSize = collidable.size(); i < collidableSize; i++) {
        Collidable colObj = collidable.get(i);
        Body3D body = ifDynamic(colObj);
        if (body != null) {
            body.setHitFraction(1f);
            if (body.isActive() && (!body.isStaticOrKinematicObject())) {
                body.predictIntegratedTransform(timeStep, predictedTrans);
                Transform BW = body.transform;
                tmp.sub(predictedTrans, BW);
                float squareMotion = tmp.lengthSquared();
                float motionThresh = body.getCcdSquareMotionThreshold();
                if (motionThresh != 0f && motionThresh < squareMotion) {
                    if (body.shape().isConvex()) {
                        BulletStats.gNumClampedCcdMotions++;
                        ClosestNotMeConvexResultCallback sweepResults = new ClosestNotMeConvexResultCallback(body, BW, predictedTrans, broadphase.getOverlappingPairCache(), intersecter);
                        // ConvexShape convexShape = (ConvexShape)body.getCollisionShape();
                        // btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
                        tmpSphere.setRadius(body.getCcdSweptSphereRadius());
                        Broadphasing bph = body.broadphase;
                        sweepResults.collisionFilterGroup = bph.collisionFilterGroup;
                        sweepResults.collisionFilterMask = bph.collisionFilterMask;
                        convexSweepTest(tmpSphere, BW, predictedTrans, sweepResults);
                        // JAVA NOTE: added closestHitFraction test to prevent objects being stuck
                        if (sweepResults.hasHit() && (sweepResults.closestHitFraction > 0.0001f)) {
                            body.setHitFraction(sweepResults.closestHitFraction);
                            body.predictIntegratedTransform(timeStep * body.getHitFraction(), predictedTrans);
                            body.setHitFraction(0f);
                        // System.out.printf("clamped integration to hit fraction = %f\n", sweepResults.closestHitFraction);
                        }
                    }
                }
                body.proceedToTransform(predictedTrans);
            }
        }
    }
}