Examples with Transform com.bulletphysics.linearmath.Transform used on opensource projects

Example 11 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class ConvexPlaneCollisionAlgorithm method processCollision.

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

Example 12 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class ConvexTriangleCallback method setTimeStepAndCounters.

public void setTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    this.dispatchInfoPtr = dispatchInfo;
    this.collisionMarginTriangle = collisionMarginTriangle;
    this.resultOut = resultOut;
    Stack stack = Stack.enter();
    // recalc aabbs
    Transform convexInTriangleSpace = stack.allocTransform();
    triBody.getWorldTransform(convexInTriangleSpace);
    convexInTriangleSpace.inverse();
    convexInTriangleSpace.mul(convexBody.getWorldTransform(stack.allocTransform()));
    CollisionShape convexShape = (CollisionShape) convexBody.getCollisionShape();
    //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
    convexShape.getAabb(convexInTriangleSpace, aabbMin, aabbMax);
    float extraMargin = collisionMarginTriangle;
    Vector3f extra = stack.allocVector3f();
    extra.set(extraMargin, extraMargin, extraMargin);
    aabbMax.add(extra);
    aabbMin.sub(extra);
    stack.leave();
}

Example 13 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class SphereSphereCollisionAlgorithm method processCollision.

@Override
public void processCollision(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    if (manifoldPtr == null) {
        return;
    }
    Stack stack = Stack.enter();
    Transform tmpTrans1 = stack.allocTransform();
    Transform tmpTrans2 = stack.allocTransform();
    resultOut.setPersistentManifold(manifoldPtr);
    SphereShape sphere0 = (SphereShape) col0.getCollisionShape();
    SphereShape sphere1 = (SphereShape) col1.getCollisionShape();
    Vector3f diff = stack.allocVector3f();
    diff.sub(col0.getWorldTransform(tmpTrans1).origin, col1.getWorldTransform(tmpTrans2).origin);
    float len = diff.length();
    float radius0 = sphere0.getRadius();
    float radius1 = sphere1.getRadius();
    // if distance positive, don't generate a new contact
    if (len > (radius0 + radius1)) {
        //#ifndef CLEAR_MANIFOLD
        resultOut.refreshContactPoints();
        //#endif //CLEAR_MANIFOLD
        return;
    }
    // distance (negative means penetration)
    float dist = len - (radius0 + radius1);
    Vector3f normalOnSurfaceB = stack.allocVector3f();
    normalOnSurfaceB.set(1f, 0f, 0f);
    if (len > BulletGlobals.FLT_EPSILON) {
        normalOnSurfaceB.scale(1f / len, diff);
    }
    Vector3f tmp = stack.allocVector3f();
    // point on A (worldspace)
    Vector3f pos0 = stack.allocVector3f();
    tmp.scale(radius0, normalOnSurfaceB);
    pos0.sub(col0.getWorldTransform(tmpTrans1).origin, tmp);
    // point on B (worldspace)
    Vector3f pos1 = stack.allocVector3f();
    tmp.scale(radius1, normalOnSurfaceB);
    pos1.add(col1.getWorldTransform(tmpTrans2).origin, tmp);
    // report a contact. internally this will be kept persistent, and contact reduction is done
    resultOut.addContactPoint(normalOnSurfaceB, pos1, dist);
    //#ifndef CLEAR_MANIFOLD
    resultOut.refreshContactPoints();
    //#endif //CLEAR_MANIFOLD
    stack.leave();
}

Example 14 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class SubsimplexConvexCast method calcTimeOfImpact.

public boolean calcTimeOfImpact(Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    simplexSolver.reset();
    Vector3f linVelA = stack.allocVector3f();
    Vector3f linVelB = stack.allocVector3f();
    linVelA.sub(toA.origin, fromA.origin);
    linVelB.sub(toB.origin, fromB.origin);
    float lambda = 0f;
    Transform interpolatedTransA = stack.alloc(fromA);
    Transform interpolatedTransB = stack.alloc(fromB);
    // take relative motion
    Vector3f r = stack.allocVector3f();
    r.sub(linVelA, linVelB);
    Vector3f v = stack.allocVector3f();
    tmp.negate(r);
    MatrixUtil.transposeTransform(tmp, tmp, fromA.basis);
    Vector3f supVertexA = convexA.localGetSupportingVertex(tmp, stack.allocVector3f());
    fromA.transform(supVertexA);
    MatrixUtil.transposeTransform(tmp, r, fromB.basis);
    Vector3f supVertexB = convexB.localGetSupportingVertex(tmp, stack.allocVector3f());
    fromB.transform(supVertexB);
    v.sub(supVertexA, supVertexB);
    int maxIter = MAX_ITERATIONS;
    Vector3f n = stack.allocVector3f();
    n.set(0f, 0f, 0f);
    boolean hasResult = false;
    Vector3f c = stack.allocVector3f();
    float lastLambda = lambda;
    float dist2 = v.lengthSquared();
    //#ifdef BT_USE_DOUBLE_PRECISION
    //	btScalar epsilon = btScalar(0.0001);
    //#else
    float epsilon = 0.0001f;
    //#endif
    Vector3f w = stack.allocVector3f(), p = stack.allocVector3f();
    float VdotR;
    while ((dist2 > epsilon) && (maxIter--) != 0) {
        tmp.negate(v);
        MatrixUtil.transposeTransform(tmp, tmp, interpolatedTransA.basis);
        convexA.localGetSupportingVertex(tmp, supVertexA);
        interpolatedTransA.transform(supVertexA);
        MatrixUtil.transposeTransform(tmp, v, interpolatedTransB.basis);
        convexB.localGetSupportingVertex(tmp, supVertexB);
        interpolatedTransB.transform(supVertexB);
        w.sub(supVertexA, supVertexB);
        float VdotW = v.dot(w);
        if (lambda > 1f) {
            stack.leave();
            return false;
        }
        if (VdotW > 0f) {
            VdotR = v.dot(r);
            if (VdotR >= -(BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
                stack.leave();
                return false;
            } else {
                lambda = lambda - VdotW / VdotR;
                // interpolate to next lambda
                //	x = s + lambda * r;
                VectorUtil.setInterpolate3(interpolatedTransA.origin, fromA.origin, toA.origin, lambda);
                VectorUtil.setInterpolate3(interpolatedTransB.origin, fromB.origin, toB.origin, lambda);
                //m_simplexSolver->reset();
                // check next line
                w.sub(supVertexA, supVertexB);
                lastLambda = lambda;
                n.set(v);
                hasResult = true;
            }
        }
        simplexSolver.addVertex(w, supVertexA, supVertexB);
        if (simplexSolver.closest(v)) {
            dist2 = v.lengthSquared();
            hasResult = true;
        // todo: check this normal for validity
        //n.set(v);
        //printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
        //printf("DIST2=%f\n",dist2);
        //printf("numverts = %i\n",m_simplexSolver->numVertices());
        } else {
            dist2 = 0f;
        }
    }
    //int numiter = MAX_ITERATIONS - maxIter;
    //	printf("number of iterations: %d", numiter);
    // don't report a time of impact when moving 'away' from the hitnormal
    result.fraction = lambda;
    if (n.lengthSquared() >= BulletGlobals.SIMD_EPSILON * BulletGlobals.SIMD_EPSILON) {
        result.normal.normalize(n);
    } else {
        result.normal.set(0f, 0f, 0f);
    }
    // don't report time of impact for motion away from the contact normal (or causes minor penetration)
    if (result.normal.dot(r) >= -result.allowedPenetration) {
        stack.leave();
        return false;
    }
    Vector3f hitA = stack.allocVector3f();
    Vector3f hitB = stack.allocVector3f();
    simplexSolver.compute_points(hitA, hitB);
    result.hitPoint.set(hitB);
    stack.leave();
    return true;
}

Example 15 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class CollisionShape method getBoundingSphere.

public float getBoundingSphere(Vector3f center) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    Transform tr = stack.allocTransform();
    tr.setIdentity();
    Vector3f aabbMin = stack.allocVector3f(), aabbMax = stack.allocVector3f();
    getAabb(tr, aabbMin, aabbMax);
    tmp.sub(aabbMax, aabbMin);
    float radius = tmp.length() * 0.5f;
    tmp.add(aabbMin, aabbMax);
    center.scale(0.5f, tmp);
    stack.leave();
    return radius;
}