Examples with Vector3f javax.vecmath.Vector3f used on opensource projects

Example 16 with Vector3f

use of javax.vecmath.Vector3f in project bdx by GoranM.

the class CollisionWorld method convexSweepTest.

/**
	 * convexTest performs a swept convex cast on all objects in the {@link CollisionWorld}, and calls the resultCallback
	 * This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
	 */
public void convexSweepTest(ConvexShape castShape, Transform convexFromWorld, Transform convexToWorld, ConvexResultCallback resultCallback) {
    Stack stack = Stack.enter();
    Transform convexFromTrans = stack.allocTransform();
    Transform convexToTrans = stack.allocTransform();
    convexFromTrans.set(convexFromWorld);
    convexToTrans.set(convexToWorld);
    Vector3f castShapeAabbMin = stack.allocVector3f();
    Vector3f castShapeAabbMax = stack.allocVector3f();
    // Compute AABB that encompasses angular movement
    {
        Vector3f linVel = stack.allocVector3f();
        Vector3f angVel = stack.allocVector3f();
        TransformUtil.calculateVelocity(convexFromTrans, convexToTrans, 1f, linVel, angVel);
        Transform R = stack.allocTransform();
        R.setIdentity();
        R.setRotation(convexFromTrans.getRotation(stack.allocQuat4f()));
        castShape.calculateTemporalAabb(R, linVel, angVel, 1f, castShapeAabbMin, castShapeAabbMax);
    }
    Transform tmpTrans = stack.allocTransform();
    Vector3f collisionObjectAabbMin = stack.allocVector3f();
    Vector3f collisionObjectAabbMax = stack.allocVector3f();
    float[] hitLambda = new float[1];
    // do a ray-shape query using convexCaster (CCD)
    for (int i = 0; i < collisionObjects.size(); i++) {
        CollisionObject collisionObject = collisionObjects.getQuick(i);
        // only perform raycast if filterMask matches
        if (resultCallback.needsCollision(collisionObject.getBroadphaseHandle())) {
            //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
            collisionObject.getWorldTransform(tmpTrans);
            collisionObject.getCollisionShape().getAabb(tmpTrans, collisionObjectAabbMin, collisionObjectAabbMax);
            AabbUtil2.aabbExpand(collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
            // could use resultCallback.closestHitFraction, but needs testing
            hitLambda[0] = 1f;
            Vector3f hitNormal = stack.allocVector3f();
            if (AabbUtil2.rayAabb(convexFromWorld.origin, convexToWorld.origin, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) {
                objectQuerySingle(castShape, convexFromTrans, convexToTrans, collisionObject, collisionObject.getCollisionShape(), tmpTrans, resultCallback, getDispatchInfo().allowedCcdPenetration);
            }
        }
    }
    stack.leave();
}

Example 17 with Vector3f

use of javax.vecmath.Vector3f in project bdx by GoranM.

the class CollisionWorld 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 void rayTest(Vector3f rayFromWorld, Vector3f rayToWorld, RayResultCallback resultCallback) {
    Stack stack = Stack.enter();
    Transform rayFromTrans = stack.allocTransform(), rayToTrans = stack.allocTransform();
    rayFromTrans.setIdentity();
    rayFromTrans.origin.set(rayFromWorld);
    rayToTrans.setIdentity();
    rayToTrans.origin.set(rayToWorld);
    // go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
    Vector3f collisionObjectAabbMin = stack.allocVector3f(), collisionObjectAabbMax = stack.allocVector3f();
    float[] hitLambda = new float[1];
    Transform tmpTrans = stack.allocTransform();
    for (int i = 0; i < collisionObjects.size(); i++) {
        // terminate further ray tests, once the closestHitFraction reached zero
        if (resultCallback.closestHitFraction == 0f) {
            break;
        }
        CollisionObject collisionObject = collisionObjects.getQuick(i);
        // only perform raycast if filterMask matches
        if (resultCallback.needsCollision(collisionObject.getBroadphaseHandle())) {
            //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
            collisionObject.getCollisionShape().getAabb(collisionObject.getWorldTransform(tmpTrans), collisionObjectAabbMin, collisionObjectAabbMax);
            hitLambda[0] = resultCallback.closestHitFraction;
            Vector3f hitNormal = stack.allocVector3f();
            if (AabbUtil2.rayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) {
                rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionObject.getCollisionShape(), collisionObject.getWorldTransform(tmpTrans), resultCallback);
            }
        }
    }
    stack.leave();
}

Example 18 with Vector3f

use of javax.vecmath.Vector3f in project bdx by GoranM.

the class CollisionWorld method objectQuerySingle.

/**
	 * objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
	 */
public static void objectQuerySingle(ConvexShape castShape, Transform convexFromTrans, Transform convexToTrans, CollisionObject collisionObject, CollisionShape collisionShape, Transform colObjWorldTransform, ConvexResultCallback resultCallback, float allowedPenetration) {
    Stack stack = Stack.enter();
    if (collisionShape.isConvex()) {
        CastResult castResult = new CastResult();
        castResult.allowedPenetration = allowedPenetration;
        // ??
        castResult.fraction = 1f;
        ConvexShape convexShape = (ConvexShape) collisionShape;
        VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
        GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = new GjkEpaPenetrationDepthSolver();
        // JAVA TODO: should be convexCaster1
        //ContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
        GjkConvexCast convexCaster2 = new GjkConvexCast(castShape, convexShape, simplexSolver);
        //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
        ConvexCast castPtr = convexCaster2;
        if (castPtr.calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) {
            // add hit
            if (castResult.normal.lengthSquared() > 0.0001f) {
                if (castResult.fraction < resultCallback.closestHitFraction) {
                    castResult.normal.normalize();
                    LocalConvexResult localConvexResult = new LocalConvexResult(collisionObject, null, castResult.normal, castResult.hitPoint, castResult.fraction);
                    boolean normalInWorldSpace = true;
                    resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
                }
            }
        }
    } else {
        if (collisionShape.isConcave()) {
            if (collisionShape.getShapeType() == BroadphaseNativeType.TRIANGLE_MESH_SHAPE_PROXYTYPE) {
                BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape) collisionShape;
                Transform worldTocollisionObject = stack.allocTransform();
                worldTocollisionObject.inverse(colObjWorldTransform);
                Vector3f convexFromLocal = stack.allocVector3f();
                convexFromLocal.set(convexFromTrans.origin);
                worldTocollisionObject.transform(convexFromLocal);
                Vector3f convexToLocal = stack.allocVector3f();
                convexToLocal.set(convexToTrans.origin);
                worldTocollisionObject.transform(convexToLocal);
                // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
                Transform rotationXform = stack.allocTransform();
                Matrix3f tmpMat = stack.allocMatrix3f();
                tmpMat.mul(worldTocollisionObject.basis, convexToTrans.basis);
                rotationXform.set(tmpMat);
                BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, convexFromTrans, convexToTrans, resultCallback, collisionObject, triangleMesh, colObjWorldTransform);
                tccb.hitFraction = resultCallback.closestHitFraction;
                tccb.normalInWorldSpace = true;
                Vector3f boxMinLocal = stack.allocVector3f();
                Vector3f boxMaxLocal = stack.allocVector3f();
                castShape.getAabb(rotationXform, boxMinLocal, boxMaxLocal);
                triangleMesh.performConvexcast(tccb, convexFromLocal, convexToLocal, boxMinLocal, boxMaxLocal);
            } else {
                BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape) collisionShape;
                Transform worldTocollisionObject = stack.allocTransform();
                worldTocollisionObject.inverse(colObjWorldTransform);
                Vector3f convexFromLocal = stack.allocVector3f();
                convexFromLocal.set(convexFromTrans.origin);
                worldTocollisionObject.transform(convexFromLocal);
                Vector3f convexToLocal = stack.allocVector3f();
                convexToLocal.set(convexToTrans.origin);
                worldTocollisionObject.transform(convexToLocal);
                // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
                Transform rotationXform = stack.allocTransform();
                Matrix3f tmpMat = stack.allocMatrix3f();
                tmpMat.mul(worldTocollisionObject.basis, convexToTrans.basis);
                rotationXform.set(tmpMat);
                BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, convexFromTrans, convexToTrans, resultCallback, collisionObject, triangleMesh, colObjWorldTransform);
                tccb.hitFraction = resultCallback.closestHitFraction;
                tccb.normalInWorldSpace = false;
                Vector3f boxMinLocal = stack.allocVector3f();
                Vector3f boxMaxLocal = stack.allocVector3f();
                castShape.getAabb(rotationXform, boxMinLocal, boxMaxLocal);
                Vector3f rayAabbMinLocal = stack.alloc(convexFromLocal);
                VectorUtil.setMin(rayAabbMinLocal, convexToLocal);
                Vector3f rayAabbMaxLocal = stack.alloc(convexFromLocal);
                VectorUtil.setMax(rayAabbMaxLocal, convexToLocal);
                rayAabbMinLocal.add(boxMinLocal);
                rayAabbMaxLocal.add(boxMaxLocal);
                triangleMesh.processAllTriangles(tccb, rayAabbMinLocal, rayAabbMaxLocal);
            }
        } else {
            // todo: use AABB tree or other BVH acceleration structure!
            if (collisionShape.isCompound()) {
                CompoundShape compoundShape = (CompoundShape) collisionShape;
                for (int i = 0; i < compoundShape.getNumChildShapes(); i++) {
                    Transform childTrans = compoundShape.getChildTransform(i, stack.allocTransform());
                    CollisionShape childCollisionShape = compoundShape.getChildShape(i);
                    Transform childWorldTrans = stack.allocTransform();
                    childWorldTrans.mul(colObjWorldTransform, childTrans);
                    // replace collision shape so that callback can determine the triangle
                    CollisionShape saveCollisionShape = collisionObject.getCollisionShape();
                    collisionObject.internalSetTemporaryCollisionShape(childCollisionShape);
                    objectQuerySingle(castShape, convexFromTrans, convexToTrans, collisionObject, childCollisionShape, childWorldTrans, resultCallback, allowedPenetration);
                    // restore
                    collisionObject.internalSetTemporaryCollisionShape(saveCollisionShape);
                }
            }
        }
    }
    stack.leave();
}

Example 19 with Vector3f

use of javax.vecmath.Vector3f in project bdx by GoranM.

the class CollisionWorld method addCollisionObject.

public void addCollisionObject(CollisionObject collisionObject, short collisionFilterGroup, short collisionFilterMask) {
    // check that the object isn't already added
    assert (!collisionObjects.contains(collisionObject));
    Stack stack = Stack.enter();
    collisionObjects.add(collisionObject);
    // calculate new AABB
    // TODO: check if it's overwritten or not
    Transform trans = collisionObject.getWorldTransform(stack.allocTransform());
    Vector3f minAabb = stack.allocVector3f();
    Vector3f maxAabb = stack.allocVector3f();
    collisionObject.getCollisionShape().getAabb(trans, minAabb, maxAabb);
    BroadphaseNativeType type = collisionObject.getCollisionShape().getShapeType();
    collisionObject.setBroadphaseHandle(getBroadphase().createProxy(minAabb, maxAabb, type, collisionObject, collisionFilterGroup, collisionFilterMask, dispatcher1, null));
    stack.leave();
}

Example 20 with Vector3f

use of javax.vecmath.Vector3f in project bdx by GoranM.

the class ConvexConvexAlgorithm method calculateTimeOfImpact.

@Override
public float calculateTimeOfImpact(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    Transform tmpTrans1 = stack.allocTransform();
    Transform tmpTrans2 = stack.allocTransform();
    // 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).origin, col0.getWorldTransform(tmpTrans2).origin);
    float squareMot0 = tmp.lengthSquared();
    tmp.sub(col1.getInterpolationWorldTransform(tmpTrans1).origin, col1.getWorldTransform(tmpTrans2).origin);
    float squareMot1 = tmp.lengthSquared();
    if (squareMot0 < col0.getCcdSquareMotionThreshold() && squareMot1 < col1.getCcdSquareMotionThreshold()) {
        return resultFraction;
    }
    if (disableCcd) {
        stack.leave();
        return 1f;
    }
    Transform tmpTrans3 = stack.allocTransform();
    Transform tmpTrans4 = stack.allocTransform();
    // 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.getCollisionShape();
        // todo: allow non-zero sphere sizes, for better approximation
        SphereShape sphere1 = new SphereShape(col1.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(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.getCollisionShape();
        // 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;
            }
        }
    }
    stack.leave();
    return resultFraction;
}