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

Example 21 with Vector3f

use of javax.vecmath.Vector3f 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 22 with Vector3f

use of javax.vecmath.Vector3f 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 23 with Vector3f

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

the class ManifoldResult method addContactPoint.

public void addContactPoint(Vector3f normalOnBInWorld, Vector3f pointInWorld, float depth) {
    assert (manifoldPtr != null);
    if (depth > manifoldPtr.getContactBreakingThreshold()) {
        return;
    }
    boolean isSwapped = manifoldPtr.getBody0() != body0;
    Stack stack = Stack.enter();
    Vector3f pointA = stack.allocVector3f();
    pointA.scaleAdd(depth, normalOnBInWorld, pointInWorld);
    Vector3f localA = stack.allocVector3f();
    Vector3f localB = stack.allocVector3f();
    if (isSwapped) {
        rootTransB.invXform(pointA, localA);
        rootTransA.invXform(pointInWorld, localB);
    } else {
        rootTransA.invXform(pointA, localA);
        rootTransB.invXform(pointInWorld, localB);
    }
    ManifoldPoint newPt = pointsPool.get();
    newPt.init(localA, localB, normalOnBInWorld, depth);
    newPt.positionWorldOnA.set(pointA);
    newPt.positionWorldOnB.set(pointInWorld);
    int insertIndex = manifoldPtr.getCacheEntry(newPt);
    newPt.combinedFriction = calculateCombinedFriction(body0, body1);
    newPt.combinedRestitution = calculateCombinedRestitution(body0, body1);
    // BP mod, store contact triangles.
    newPt.partId0 = partId0;
    newPt.partId1 = partId1;
    newPt.index0 = index0;
    newPt.index1 = index1;
    /// todo, check this for any side effects
    if (insertIndex >= 0) {
        //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
        manifoldPtr.replaceContactPoint(newPt, insertIndex);
    } else {
        insertIndex = manifoldPtr.addManifoldPoint(newPt);
    }
    // User can override friction and/or restitution
    if (BulletGlobals.getContactAddedCallback() != null && // and if either of the two bodies requires custom material
    ((body0.getCollisionFlags() & CollisionFlags.CUSTOM_MATERIAL_CALLBACK) != 0 || (body1.getCollisionFlags() & CollisionFlags.CUSTOM_MATERIAL_CALLBACK) != 0)) {
        //experimental feature info, for per-triangle material etc.
        CollisionObject obj0 = isSwapped ? body1 : body0;
        CollisionObject obj1 = isSwapped ? body0 : body1;
        BulletGlobals.getContactAddedCallback().contactAdded(manifoldPtr.getContactPoint(insertIndex), obj0, partId0, index0, obj1, partId1, index1);
    }
    pointsPool.release(newPt);
    stack.leave();
}

Example 24 with Vector3f

use of javax.vecmath.Vector3f 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 25 with Vector3f

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

the class PersistentManifold method refreshContactPoints.

/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
public void refreshContactPoints(Transform trA, Transform trB) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    int i;
    // first refresh worldspace positions and distance
    for (i = getNumContacts() - 1; i >= 0; i--) {
        ManifoldPoint manifoldPoint = pointCache[i];
        manifoldPoint.positionWorldOnA.set(manifoldPoint.localPointA);
        trA.transform(manifoldPoint.positionWorldOnA);
        manifoldPoint.positionWorldOnB.set(manifoldPoint.localPointB);
        trB.transform(manifoldPoint.positionWorldOnB);
        tmp.set(manifoldPoint.positionWorldOnA);
        tmp.sub(manifoldPoint.positionWorldOnB);
        manifoldPoint.distance1 = tmp.dot(manifoldPoint.normalWorldOnB);
        manifoldPoint.lifeTime++;
    }
    // then 
    float distance2d;
    Vector3f projectedDifference = stack.allocVector3f(), projectedPoint = stack.allocVector3f();
    for (i = getNumContacts() - 1; i >= 0; i--) {
        ManifoldPoint manifoldPoint = pointCache[i];
        // contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
        if (!validContactDistance(manifoldPoint)) {
            removeContactPoint(i);
        } else {
            // contact also becomes invalid when relative movement orthogonal to normal exceeds margin
            tmp.scale(manifoldPoint.distance1, manifoldPoint.normalWorldOnB);
            projectedPoint.sub(manifoldPoint.positionWorldOnA, tmp);
            projectedDifference.sub(manifoldPoint.positionWorldOnB, projectedPoint);
            distance2d = projectedDifference.dot(projectedDifference);
            if (distance2d > getContactBreakingThreshold() * getContactBreakingThreshold()) {
                removeContactPoint(i);
            } else {
                // contact point processed callback
                if (BulletGlobals.getContactProcessedCallback() != null) {
                    BulletGlobals.getContactProcessedCallback().contactProcessed(manifoldPoint, body0, body1);
                }
            }
        }
    }
    //#ifdef DEBUG_PERSISTENCY
    //	DebugPersistency();
    //#endif //
    stack.leave();
}