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

Example 51 with Transform

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

the class CompoundShape method calculatePrincipalAxisTransform.

/**
	 * Computes the exact moment of inertia and the transform from the coordinate
	 * system defined by the principal axes of the moment of inertia and the center
	 * of mass to the current coordinate system. "masses" points to an array
	 * of masses of the children. The resulting transform "principal" has to be
	 * applied inversely to all children transforms in order for the local coordinate
	 * system of the compound shape to be centered at the center of mass and to coincide
	 * with the principal axes. This also necessitates a correction of the world transform
	 * of the collision object by the principal transform.
	 */
public void calculatePrincipalAxisTransform(float[] masses, Transform principal, Vector3f inertia) {
    int n = children.size();
    Stack stack = Stack.enter();
    float totalMass = 0;
    Vector3f center = stack.allocVector3f();
    center.set(0, 0, 0);
    for (int k = 0; k < n; k++) {
        center.scaleAdd(masses[k], children.getQuick(k).transform.origin, center);
        totalMass += masses[k];
    }
    center.scale(1f / totalMass);
    principal.origin.set(center);
    Matrix3f tensor = stack.allocMatrix3f();
    tensor.setZero();
    for (int k = 0; k < n; k++) {
        Vector3f i = stack.allocVector3f();
        children.getQuick(k).childShape.calculateLocalInertia(masses[k], i);
        Transform t = children.getQuick(k).transform;
        Vector3f o = stack.allocVector3f();
        o.sub(t.origin, center);
        // compute inertia tensor in coordinate system of compound shape
        Matrix3f j = stack.allocMatrix3f();
        j.transpose(t.basis);
        j.m00 *= i.x;
        j.m01 *= i.x;
        j.m02 *= i.x;
        j.m10 *= i.y;
        j.m11 *= i.y;
        j.m12 *= i.y;
        j.m20 *= i.z;
        j.m21 *= i.z;
        j.m22 *= i.z;
        j.mul(t.basis, j);
        // add inertia tensor
        tensor.add(j);
        // compute inertia tensor of pointmass at o
        float o2 = o.lengthSquared();
        j.setRow(0, o2, 0, 0);
        j.setRow(1, 0, o2, 0);
        j.setRow(2, 0, 0, o2);
        j.m00 += o.x * -o.x;
        j.m01 += o.y * -o.x;
        j.m02 += o.z * -o.x;
        j.m10 += o.x * -o.y;
        j.m11 += o.y * -o.y;
        j.m12 += o.z * -o.y;
        j.m20 += o.x * -o.z;
        j.m21 += o.y * -o.z;
        j.m22 += o.z * -o.z;
        // add inertia tensor of pointmass
        tensor.m00 += masses[k] * j.m00;
        tensor.m01 += masses[k] * j.m01;
        tensor.m02 += masses[k] * j.m02;
        tensor.m10 += masses[k] * j.m10;
        tensor.m11 += masses[k] * j.m11;
        tensor.m12 += masses[k] * j.m12;
        tensor.m20 += masses[k] * j.m20;
        tensor.m21 += masses[k] * j.m21;
        tensor.m22 += masses[k] * j.m22;
    }
    MatrixUtil.diagonalize(tensor, principal.basis, 0.00001f, 20);
    inertia.set(tensor.m00, tensor.m11, tensor.m22);
    stack.leave();
}

Example 52 with Transform

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

the class CompoundShape method calculateLocalInertia.

@Override
public void calculateLocalInertia(float mass, Vector3f inertia) {
    // approximation: take the inertia from the aabb for now
    Stack stack = Stack.enter();
    Transform ident = stack.allocTransform();
    ident.setIdentity();
    Vector3f aabbMin = stack.allocVector3f(), aabbMax = stack.allocVector3f();
    getAabb(ident, aabbMin, aabbMax);
    Vector3f halfExtents = stack.allocVector3f();
    halfExtents.sub(aabbMax, aabbMin);
    halfExtents.scale(0.5f);
    float lx = 2f * halfExtents.x;
    float ly = 2f * halfExtents.y;
    float lz = 2f * halfExtents.z;
    inertia.x = (mass / 12f) * (ly * ly + lz * lz);
    inertia.y = (mass / 12f) * (lx * lx + lz * lz);
    inertia.z = (mass / 12f) * (lx * lx + ly * ly);
    stack.leave();
}

Example 53 with Transform

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

the class DiscreteDynamicsWorld method debugDrawWorld.

@Override
public void debugDrawWorld() {
    Stack stack = Stack.enter();
    if (getDebugDrawer() != null && (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_CONTACT_POINTS) != 0) {
        int numManifolds = getDispatcher().getNumManifolds();
        Vector3f color = stack.allocVector3f();
        color.set(0f, 0f, 0f);
        for (int i = 0; i < numManifolds; i++) {
            PersistentManifold contactManifold = getDispatcher().getManifoldByIndexInternal(i);
            //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
            //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
            int numContacts = contactManifold.getNumContacts();
            for (int j = 0; j < numContacts; j++) {
                ManifoldPoint cp = contactManifold.getContactPoint(j);
                getDebugDrawer().drawContactPoint(cp.positionWorldOnB, cp.normalWorldOnB, cp.getDistance(), cp.getLifeTime(), color);
            }
        }
    }
    if (getDebugDrawer() != null && (getDebugDrawer().getDebugMode() & (DebugDrawModes.DRAW_WIREFRAME | DebugDrawModes.DRAW_AABB)) != 0) {
        int i;
        Transform tmpTrans = stack.allocTransform();
        Vector3f minAabb = stack.allocVector3f();
        Vector3f maxAabb = stack.allocVector3f();
        Vector3f colorvec = stack.allocVector3f();
        // todo: iterate over awake simulation islands!
        for (i = 0; i < collisionObjects.size(); i++) {
            CollisionObject colObj = collisionObjects.getQuick(i);
            if (getDebugDrawer() != null && (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_WIREFRAME) != 0) {
                Vector3f color = stack.allocVector3f();
                color.set(255f, 255f, 255f);
                switch(colObj.getActivationState()) {
                    case CollisionObject.ACTIVE_TAG:
                        color.set(255f, 255f, 255f);
                        break;
                    case CollisionObject.ISLAND_SLEEPING:
                        color.set(0f, 255f, 0f);
                        break;
                    case CollisionObject.WANTS_DEACTIVATION:
                        color.set(0f, 255f, 255f);
                        break;
                    case CollisionObject.DISABLE_DEACTIVATION:
                        color.set(255f, 0f, 0f);
                        break;
                    case CollisionObject.DISABLE_SIMULATION:
                        color.set(255f, 255f, 0f);
                        break;
                    default:
                        {
                            color.set(255f, 0f, 0f);
                        }
                }
                debugDrawObject(colObj.getWorldTransform(tmpTrans), colObj.getCollisionShape(), color);
            }
            if (debugDrawer != null && (debugDrawer.getDebugMode() & DebugDrawModes.DRAW_AABB) != 0) {
                colorvec.set(1f, 0f, 0f);
                colObj.getCollisionShape().getAabb(colObj.getWorldTransform(tmpTrans), minAabb, maxAabb);
                debugDrawer.drawAabb(minAabb, maxAabb, colorvec);
            }
        }
        Vector3f wheelColor = stack.allocVector3f();
        Vector3f wheelPosWS = stack.allocVector3f();
        Vector3f axle = stack.allocVector3f();
        Vector3f tmp = stack.allocVector3f();
        for (i = 0; i < vehicles.size(); i++) {
            for (int v = 0; v < vehicles.getQuick(i).getNumWheels(); v++) {
                wheelColor.set(0, 255, 255);
                if (vehicles.getQuick(i).getWheelInfo(v).raycastInfo.isInContact) {
                    wheelColor.set(0, 0, 255);
                } else {
                    wheelColor.set(255, 0, 255);
                }
                wheelPosWS.set(vehicles.getQuick(i).getWheelInfo(v).worldTransform.origin);
                axle.set(vehicles.getQuick(i).getWheelInfo(v).worldTransform.basis.getElement(0, vehicles.getQuick(i).getRightAxis()), vehicles.getQuick(i).getWheelInfo(v).worldTransform.basis.getElement(1, vehicles.getQuick(i).getRightAxis()), vehicles.getQuick(i).getWheelInfo(v).worldTransform.basis.getElement(2, vehicles.getQuick(i).getRightAxis()));
                //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
                //debug wheels (cylinders)
                tmp.add(wheelPosWS, axle);
                debugDrawer.drawLine(wheelPosWS, tmp, wheelColor);
                debugDrawer.drawLine(wheelPosWS, vehicles.getQuick(i).getWheelInfo(v).raycastInfo.contactPointWS, wheelColor);
            }
        }
        if (getDebugDrawer() != null && getDebugDrawer().getDebugMode() != 0) {
            for (i = 0; i < actions.size(); i++) {
                actions.getQuick(i).debugDraw(debugDrawer);
            }
        }
    }
    stack.leave();
}

Example 54 with Transform

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

the class DiscreteDynamicsWorld method debugDrawObject.

public void debugDrawObject(Transform worldTransform, CollisionShape shape, Vector3f color) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    Vector3f tmp2 = stack.allocVector3f();
    // Draw a small simplex at the center of the object
    {
        Vector3f start = stack.alloc(worldTransform.origin);
        tmp.set(1f, 0f, 0f);
        worldTransform.basis.transform(tmp);
        tmp.add(start);
        tmp2.set(1f, 0f, 0f);
        getDebugDrawer().drawLine(start, tmp, tmp2);
        tmp.set(0f, 1f, 0f);
        worldTransform.basis.transform(tmp);
        tmp.add(start);
        tmp2.set(0f, 1f, 0f);
        getDebugDrawer().drawLine(start, tmp, tmp2);
        tmp.set(0f, 0f, 1f);
        worldTransform.basis.transform(tmp);
        tmp.add(start);
        tmp2.set(0f, 0f, 1f);
        getDebugDrawer().drawLine(start, tmp, tmp2);
    }
    if (shape.getShapeType() == BroadphaseNativeType.COMPOUND_SHAPE_PROXYTYPE) {
        CompoundShape compoundShape = (CompoundShape) shape;
        for (int i = compoundShape.getNumChildShapes() - 1; i >= 0; i--) {
            Transform childTrans = new Transform();
            compoundShape.getChildTransform(i, childTrans);
            CollisionShape colShape = compoundShape.getChildShape(i);
            Transform res = new Transform();
            res.mul(worldTransform, childTrans);
            debugDrawObject(res, colShape, color);
        }
    } else {
        switch(shape.getShapeType()) {
            case SPHERE_SHAPE_PROXYTYPE:
                {
                    SphereShape sphereShape = (SphereShape) shape;
                    //radius doesn't include the margin, so draw with margin
                    float radius = sphereShape.getMargin();
                    debugDrawSphere(radius, worldTransform, color);
                    break;
                }
            //}
            case CAPSULE_SHAPE_PROXYTYPE:
                {
                    CapsuleShape capsuleShape = (CapsuleShape) shape;
                    float radius = capsuleShape.getRadius();
                    float halfHeight = capsuleShape.getHalfHeight();
                    // Draw the ends
                    {
                        Transform childTransform = new Transform(worldTransform);
                        childTransform.origin.set(0, 0, halfHeight);
                        worldTransform.transform(childTransform.origin);
                        debugDrawSphere(radius, childTransform, color);
                    }
                    {
                        Transform childTransform = new Transform(worldTransform);
                        childTransform.origin.set(0, 0, -halfHeight);
                        worldTransform.transform(childTransform.origin);
                        debugDrawSphere(radius, childTransform, color);
                    }
                    // Draw some additional lines
                    Vector3f from = stack.allocVector3f();
                    Vector3f a = stack.allocVector3f();
                    Vector3f to = stack.allocVector3f();
                    Vector3f b = stack.allocVector3f();
                    from.set(worldTransform.origin);
                    a.set(-radius, 0, halfHeight);
                    worldTransform.basis.transform(a);
                    from.add(a);
                    to.set(worldTransform.origin);
                    b.set(-radius, 0, -halfHeight);
                    worldTransform.basis.transform(b);
                    to.add(b);
                    getDebugDrawer().drawLine(from, to, color);
                    from.set(worldTransform.origin);
                    a.set(radius, 0, halfHeight);
                    worldTransform.basis.transform(a);
                    from.add(a);
                    to.set(worldTransform.origin);
                    b.set(radius, 0, -halfHeight);
                    worldTransform.basis.transform(b);
                    to.add(b);
                    getDebugDrawer().drawLine(from, to, color);
                    from.set(worldTransform.origin);
                    a.set(0, -radius, halfHeight);
                    worldTransform.basis.transform(a);
                    from.add(a);
                    to.set(worldTransform.origin);
                    b.set(0, -radius, -halfHeight);
                    worldTransform.basis.transform(b);
                    to.add(b);
                    getDebugDrawer().drawLine(from, to, color);
                    from.set(worldTransform.origin);
                    a.set(0, radius, halfHeight);
                    worldTransform.basis.transform(a);
                    from.add(a);
                    to.set(worldTransform.origin);
                    b.set(0, radius, -halfHeight);
                    worldTransform.basis.transform(b);
                    to.add(b);
                    getDebugDrawer().drawLine(from, to, color);
                    break;
                }
            case CONE_SHAPE_PROXYTYPE:
                {
                    ConeShape coneShape = (ConeShape) shape;
                    //+coneShape.getMargin();
                    float radius = coneShape.getRadius();
                    //+coneShape.getMargin();
                    float height = coneShape.getHeight();
                    int upAxis = coneShape.getConeUpIndex();
                    float[] tmpv = new float[] { 0, 0, 0 };
                    Vector3f offsetHeight = new Vector3f();
                    offsetHeight.get(tmpv);
                    tmpv[upAxis] = height * 0.5f;
                    offsetHeight.set(tmpv);
                    Vector3f offsetRadius = new Vector3f();
                    offsetRadius.get(tmpv);
                    tmpv[(upAxis + 1) % 3] = radius;
                    offsetRadius.set(tmpv);
                    Vector3f offset2Radius = new Vector3f();
                    offset2Radius.get(tmpv);
                    tmpv[(upAxis + 2) % 3] = radius;
                    offset2Radius.set(tmpv);
                    Vector3f from = stack.allocVector3f();
                    Vector3f a = stack.allocVector3f();
                    Vector3f to = stack.allocVector3f();
                    Vector3f b = stack.allocVector3f();
                    from.set(worldTransform.origin);
                    a.set(offsetHeight);
                    worldTransform.basis.transform(a);
                    from.add(a);
                    to.set(worldTransform.origin);
                    b.set(offsetHeight);
                    b.negate();
                    b.add(offsetRadius);
                    worldTransform.basis.transform(b);
                    to.add(b);
                    getDebugDrawer().drawLine(from, to, color);
                    to.set(worldTransform.origin);
                    b.set(offsetHeight);
                    b.negate();
                    b.sub(offsetRadius);
                    worldTransform.basis.transform(b);
                    to.add(b);
                    getDebugDrawer().drawLine(from, to, color);
                    to.set(worldTransform.origin);
                    b.set(offsetHeight);
                    b.negate();
                    b.add(offset2Radius);
                    worldTransform.basis.transform(b);
                    to.add(b);
                    getDebugDrawer().drawLine(from, to, color);
                    to.set(worldTransform.origin);
                    b.set(offsetHeight);
                    b.negate();
                    b.sub(offset2Radius);
                    worldTransform.basis.transform(b);
                    to.add(b);
                    getDebugDrawer().drawLine(from, to, color);
                    break;
                }
            case CYLINDER_SHAPE_PROXYTYPE:
                {
                    CylinderShape cylinder = (CylinderShape) shape;
                    int upAxis = cylinder.getUpAxis();
                    float radius = cylinder.getRadius();
                    float[] tmpv = new float[] { 0, 0, 0 };
                    Vector3f halfExtents = new Vector3f();
                    cylinder.getHalfExtentsWithMargin(halfExtents);
                    halfExtents.get(tmpv);
                    float halfHeight = tmpv[upAxis];
                    Vector3f offsetHeight = new Vector3f();
                    offsetHeight.get(tmpv);
                    tmpv[upAxis] = halfHeight;
                    offsetHeight.set(tmpv);
                    Vector3f offsetRadius = new Vector3f();
                    offsetRadius.get(tmpv);
                    tmpv[(upAxis + 1) % 3] = radius;
                    offsetRadius.set(tmpv);
                    Vector3f from = stack.allocVector3f();
                    Vector3f a = stack.allocVector3f();
                    Vector3f to = stack.allocVector3f();
                    Vector3f b = stack.allocVector3f();
                    from.set(worldTransform.origin);
                    a.set(offsetHeight);
                    a.add(offsetRadius);
                    worldTransform.basis.transform(a);
                    from.add(a);
                    to.set(worldTransform.origin);
                    b.set(offsetHeight);
                    b.negate();
                    b.add(offsetRadius);
                    worldTransform.basis.transform(b);
                    to.add(b);
                    getDebugDrawer().drawLine(from, to, color);
                    from.set(worldTransform.origin);
                    a.set(offsetHeight);
                    a.sub(offsetRadius);
                    worldTransform.basis.transform(a);
                    from.add(a);
                    to.set(worldTransform.origin);
                    b.set(offsetHeight);
                    b.negate();
                    b.sub(offsetRadius);
                    worldTransform.basis.transform(b);
                    to.add(b);
                    getDebugDrawer().drawLine(from, to, color);
                    break;
                }
            default:
                {
                    if (shape.isConcave()) {
                        //btConcaveShape* concaveMesh = (btConcaveShape*) shape;
                        ////todo pass camera, for some culling
                        //btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
                        //btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
                        //DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
                        //concaveMesh.processAllTriangles(&drawCallback,aabbMin,aabbMax);
                        ConcaveShape concaveMesh = (ConcaveShape) shape;
                        //todo: pass camera for some culling			
                        Vector3f aabbMax = stack.allocVector3f();
                        Vector3f aabbMin = stack.allocVector3f();
                        aabbMax.set(1e30f, 1e30f, 1e30f);
                        aabbMin.set(-1e30f, -1e30f, -1e30f);
                        //DebugDrawcallback drawCallback;
                        DebugDrawcallback drawCallback = new DebugDrawcallback(getDebugDrawer(), worldTransform, color);
                        concaveMesh.processAllTriangles(drawCallback, aabbMin, aabbMax);
                    }
                    /// for polyhedral shapes
                    if (shape.isPolyhedral()) {
                        PolyhedralConvexShape polyshape = (PolyhedralConvexShape) shape;
                        Vector3f a = stack.allocVector3f();
                        Vector3f b = stack.allocVector3f();
                        int i;
                        for (i = 0; i < polyshape.getNumEdges(); i++) {
                            polyshape.getEdge(i, a, b);
                            worldTransform.transform(a);
                            worldTransform.transform(b);
                            getDebugDrawer().drawLine(a, b, color);
                        }
                    }
                }
        }
    }
    stack.leave();
}

Example 55 with Transform

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

the class DiscreteDynamicsWorld method synchronizeMotionStates.

protected void synchronizeMotionStates() {
    Stack stack = Stack.enter();
    Transform interpolatedTransform = stack.allocTransform();
    Transform tmpTrans = stack.allocTransform();
    Vector3f tmpLinVel = stack.allocVector3f();
    Vector3f tmpAngVel = stack.allocVector3f();
    // todo: iterate over awake simulation islands!
    for (int i = 0; i < collisionObjects.size(); i++) {
        CollisionObject colObj = collisionObjects.getQuick(i);
        RigidBody body = RigidBody.upcast(colObj);
        if (body != null && body.getMotionState() != null && !body.isStaticOrKinematicObject()) {
            // we need to call the update at least once, even for sleeping objects
            // otherwise the 'graphics' transform never updates properly
            // so todo: add 'dirty' flag
            //if (body->getActivationState() != ISLAND_SLEEPING)
            {
                TransformUtil.integrateTransform(body.getInterpolationWorldTransform(tmpTrans), body.getInterpolationLinearVelocity(tmpLinVel), body.getInterpolationAngularVelocity(tmpAngVel), localTime * body.getHitFraction(), interpolatedTransform);
                body.getMotionState().setWorldTransform(interpolatedTransform);
            }
        }
    }
    if (getDebugDrawer() != null && (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_WIREFRAME) != 0) {
        for (int i = 0; i < vehicles.size(); i++) {
            for (int v = 0; v < vehicles.getQuick(i).getNumWheels(); v++) {
                // synchronize the wheels with the (interpolated) chassis worldtransform
                vehicles.getQuick(i).updateWheelTransform(v, true);
            }
        }
    }
    stack.leave();
}