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

Example 31 with Transform

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

the class RaycastVehicle method updateVehicle.

public void updateVehicle(float step) {
    for (int i = 0; i < getNumWheels(); i++) {
        updateWheelTransform(i, false);
    }
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    currentVehicleSpeedKmHour = 3.6f * getRigidBody().getLinearVelocity(tmp).length();
    Transform chassisTrans = getChassisWorldTransform(stack.allocTransform());
    Vector3f forwardW = stack.allocVector3f();
    forwardW.set(chassisTrans.basis.getElement(0, indexForwardAxis), chassisTrans.basis.getElement(1, indexForwardAxis), chassisTrans.basis.getElement(2, indexForwardAxis));
    if (forwardW.dot(getRigidBody().getLinearVelocity(tmp)) < 0f) {
        currentVehicleSpeedKmHour *= -1f;
    }
    //
    // simulate suspension
    //
    int i = 0;
    for (i = 0; i < wheelInfo.size(); i++) {
        float depth;
        depth = rayCast(wheelInfo.getQuick(i));
    }
    updateSuspension(step);
    for (i = 0; i < wheelInfo.size(); i++) {
        // apply suspension force
        WheelInfo wheel = wheelInfo.getQuick(i);
        float suspensionForce = wheel.wheelsSuspensionForce;
        float gMaxSuspensionForce = 6000f;
        if (suspensionForce > gMaxSuspensionForce) {
            suspensionForce = gMaxSuspensionForce;
        }
        Vector3f impulse = stack.allocVector3f();
        impulse.scale(suspensionForce * step, wheel.raycastInfo.contactNormalWS);
        Vector3f relpos = stack.allocVector3f();
        relpos.sub(wheel.raycastInfo.contactPointWS, getRigidBody().getCenterOfMassPosition(tmp));
        getRigidBody().applyImpulse(impulse, relpos);
    }
    updateFriction(step);
    for (i = 0; i < wheelInfo.size(); i++) {
        WheelInfo wheel = wheelInfo.getQuick(i);
        Vector3f relpos = stack.allocVector3f();
        relpos.sub(wheel.raycastInfo.hardPointWS, getRigidBody().getCenterOfMassPosition(tmp));
        Vector3f vel = getRigidBody().getVelocityInLocalPoint(relpos, stack.allocVector3f());
        if (wheel.raycastInfo.isInContact) {
            Transform chassisWorldTransform = getChassisWorldTransform(stack.allocTransform());
            Vector3f fwd = stack.allocVector3f();
            fwd.set(chassisWorldTransform.basis.getElement(0, indexForwardAxis), chassisWorldTransform.basis.getElement(1, indexForwardAxis), chassisWorldTransform.basis.getElement(2, indexForwardAxis));
            float proj = fwd.dot(wheel.raycastInfo.contactNormalWS);
            tmp.scale(proj, wheel.raycastInfo.contactNormalWS);
            fwd.sub(tmp);
            float proj2 = fwd.dot(vel);
            wheel.deltaRotation = (proj2 * step) / (wheel.wheelsRadius);
            wheel.rotation += wheel.deltaRotation;
        } else {
            wheel.rotation += wheel.deltaRotation;
        }
        // damping of rotation when not in contact
        wheel.deltaRotation *= 0.99f;
    }
    stack.leave();
}

Example 32 with Transform

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

the class RaycastVehicle method updateWheelTransformsWS.

public void updateWheelTransformsWS(WheelInfo wheel, boolean interpolatedTransform) {
    wheel.raycastInfo.isInContact = false;
    Stack stack = Stack.enter();
    Transform chassisTrans = getChassisWorldTransform(stack.allocTransform());
    if (interpolatedTransform && (getRigidBody().getMotionState() != null)) {
        getRigidBody().getMotionState().getWorldTransform(chassisTrans);
    }
    wheel.raycastInfo.hardPointWS.set(wheel.chassisConnectionPointCS);
    chassisTrans.transform(wheel.raycastInfo.hardPointWS);
    wheel.raycastInfo.wheelDirectionWS.set(wheel.wheelDirectionCS);
    chassisTrans.basis.transform(wheel.raycastInfo.wheelDirectionWS);
    wheel.raycastInfo.wheelAxleWS.set(wheel.wheelAxleCS);
    chassisTrans.basis.transform(wheel.raycastInfo.wheelAxleWS);
    stack.leave();
}

Example 33 with Transform

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

the class GImpactCollisionAlgorithm method gimpact_vs_concave.

public void gimpact_vs_concave(CollisionObject body0, CollisionObject body1, GImpactShapeInterface shape0, ConcaveShape shape1, boolean swapped) {
    // create the callback
    GImpactTriangleCallback tricallback = new GImpactTriangleCallback();
    tricallback.algorithm = this;
    tricallback.body0 = body0;
    tricallback.body1 = body1;
    tricallback.gimpactshape0 = shape0;
    tricallback.swapped = swapped;
    tricallback.margin = shape1.getMargin();
    // getting the trimesh AABB
    Stack stack = Stack.enter();
    Transform gimpactInConcaveSpace = stack.allocTransform();
    body1.getWorldTransform(gimpactInConcaveSpace);
    gimpactInConcaveSpace.inverse();
    gimpactInConcaveSpace.mul(body0.getWorldTransform(stack.allocTransform()));
    Vector3f minAABB = stack.allocVector3f(), maxAABB = stack.allocVector3f();
    shape0.getAabb(gimpactInConcaveSpace, minAABB, maxAABB);
    shape1.processAllTriangles(tricallback, minAABB, maxAABB);
    stack.leave();
}

Example 34 with Transform

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

the class GImpactCollisionAlgorithm method gimpacttrimeshpart_vs_plane_collision.

protected void gimpacttrimeshpart_vs_plane_collision(CollisionObject body0, CollisionObject body1, GImpactMeshShapePart shape0, StaticPlaneShape shape1, boolean swapped) {
    Stack stack = Stack.enter();
    Transform orgtrans0 = body0.getWorldTransform(stack.allocTransform());
    Transform orgtrans1 = body1.getWorldTransform(stack.allocTransform());
    StaticPlaneShape planeshape = shape1;
    Vector4f plane = stack.allocVector4f();
    PlaneShape.get_plane_equation_transformed(planeshape, orgtrans1, plane);
    // test box against plane
    AABB tribox = stack.allocAABB();
    shape0.getAabb(orgtrans0, tribox.min, tribox.max);
    tribox.increment_margin(planeshape.getMargin());
    if (tribox.plane_classify(plane) != PlaneIntersectionType.COLLIDE_PLANE) {
        return;
    }
    shape0.lockChildShapes();
    float margin = shape0.getMargin() + planeshape.getMargin();
    Vector3f vertex = stack.allocVector3f();
    Vector3f tmp = stack.allocVector3f();
    int vi = shape0.getVertexCount();
    while ((vi--) != 0) {
        shape0.getVertex(vi, vertex);
        orgtrans0.transform(vertex);
        float distance = VectorUtil.dot3(vertex, plane) - plane.w - margin;
        if (//add contact
        distance < 0f) {
            if (swapped) {
                tmp.set(-plane.x, -plane.y, -plane.z);
                addContactPoint(body1, body0, vertex, tmp, distance);
            } else {
                tmp.set(plane.x, plane.y, plane.z);
                addContactPoint(body0, body1, vertex, tmp, distance);
            }
        }
    }
    shape0.unlockChildShapes();
    stack.leave();
}

Example 35 with Transform

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

the class GImpactCollisionAlgorithm method gimpact_vs_shape.

public void gimpact_vs_shape(CollisionObject body0, CollisionObject body1, GImpactShapeInterface shape0, CollisionShape shape1, boolean swapped) {
    if (shape0.getGImpactShapeType() == ShapeType.TRIMESH_SHAPE) {
        GImpactMeshShape meshshape0 = (GImpactMeshShape) shape0;
        part0 = meshshape0.getMeshPartCount();
        while ((part0--) != 0) {
            gimpact_vs_shape(body0, body1, meshshape0.getMeshPart(part0), shape1, swapped);
        }
        return;
    }
    //#ifdef GIMPACT_VS_PLANE_COLLISION
    if (shape0.getGImpactShapeType() == ShapeType.TRIMESH_SHAPE_PART && shape1.getShapeType() == BroadphaseNativeType.STATIC_PLANE_PROXYTYPE) {
        GImpactMeshShapePart shapepart = (GImpactMeshShapePart) shape0;
        StaticPlaneShape planeshape = (StaticPlaneShape) shape1;
        gimpacttrimeshpart_vs_plane_collision(body0, body1, shapepart, planeshape, swapped);
        return;
    }
    if (shape1.isCompound()) {
        CompoundShape compoundshape = (CompoundShape) shape1;
        gimpact_vs_compoundshape(body0, body1, shape0, compoundshape, swapped);
        return;
    } else if (shape1.isConcave()) {
        ConcaveShape concaveshape = (ConcaveShape) shape1;
        gimpact_vs_concave(body0, body1, shape0, concaveshape, swapped);
        return;
    }
    Stack stack = Stack.enter();
    Transform orgtrans0 = body0.getWorldTransform(stack.allocTransform());
    Transform orgtrans1 = body1.getWorldTransform(stack.allocTransform());
    IntArrayList collided_results = new IntArrayList();
    gimpact_vs_shape_find_pairs(orgtrans0, orgtrans1, shape0, shape1, collided_results);
    if (collided_results.size() == 0) {
        return;
    }
    shape0.lockChildShapes();
    GIM_ShapeRetriever retriever0 = new GIM_ShapeRetriever(shape0);
    boolean child_has_transform0 = shape0.childrenHasTransform();
    Transform tmpTrans = stack.allocTransform();
    int i = collided_results.size();
    while ((i--) != 0) {
        int child_index = collided_results.get(i);
        if (swapped) {
            triface1 = child_index;
        } else {
            triface0 = child_index;
        }
        CollisionShape colshape0 = retriever0.getChildShape(child_index);
        if (child_has_transform0) {
            tmpTrans.mul(orgtrans0, shape0.getChildTransform(child_index));
            body0.setWorldTransform(tmpTrans);
        }
        // collide two shapes
        if (swapped) {
            shape_vs_shape_collision(body1, body0, shape1, colshape0);
        } else {
            shape_vs_shape_collision(body0, body1, colshape0, shape1);
        }
        // restore transforms
        if (child_has_transform0) {
            body0.setWorldTransform(orgtrans0);
        }
    }
    shape0.unlockChildShapes();
    stack.leave();
}