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

Example 1 with StaticPlaneShape

use of spacegraph.space3d.phys.shape.StaticPlaneShape 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();
        }
    }
}