Examples with Transform spacegraph.space3d.phys.math.Transform used on opensource projects

Example 31 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class SliderConstraint method getAncorInA.

// access for PE Solver
public v3 getAncorInA(v3 out) {
    Transform tmpTrans = new Transform();
    v3 ancorInA = out;
    ancorInA.scaleAdd((lowerLinLimit + upperLinLimit) * 0.5f, sliderAxis, realPivotAInW);
    rbA.getCenterOfMassTransform(tmpTrans);
    tmpTrans.inverse();
    tmpTrans.transform(ancorInA);
    return ancorInA;
}

Example 32 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class GjkConvexCast method calcTimeOfImpact.

@Override
public boolean calcTimeOfImpact(Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result) {
    simplexSolver.reset();
    // compute linear velocity for this interval, to interpolate
    // assume no rotation/angular velocity, assert here?
    v3 linVelA = new v3();
    v3 linVelB = new v3();
    linVelA.sub(toA, fromA);
    linVelB.sub(toB, fromB);
    float radius = 0.001f;
    float lambda = 0f;
    v3 v = new v3();
    v.set(1f, 0f, 0f);
    int maxIter = MAX_ITERATIONS;
    v3 n = new v3();
    n.set(0f, 0f, 0f);
    boolean hasResult = false;
    v3 c = new v3();
    v3 r = new v3();
    r.sub(linVelA, linVelB);
    float lastLambda = lambda;
    // btScalar epsilon = btScalar(0.001);
    int numIter = 0;
    // first solution, using GJK
    Transform identityTrans = new Transform();
    identityTrans.setIdentity();
    // result.drawCoordSystem(sphereTr);
    PointCollector pointCollector = new PointCollector();
    // penetrationDepthSolver);
    gjk.init(convexA, convexB, simplexSolver, null);
    DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
    input.init();
    // we don't use margins during CCD
    // gjk.setIgnoreMargin(true);
    input.transformA.set(fromA);
    input.transformB.set(fromB);
    gjk.getClosestPoints(input, pointCollector);
    hasResult = pointCollector.hasResult;
    c.set(pointCollector.pointInWorld);
    if (hasResult) {
        float dist;
        dist = pointCollector.distance;
        n.set(pointCollector.normalOnBInWorld);
        // not close enough
        while (dist > radius) {
            numIter++;
            if (numIter > maxIter) {
                // todo: report a failure
                return false;
            }
            float dLambda = 0f;
            float projectedLinearVelocity = r.dot(n);
            dLambda = dist / (projectedLinearVelocity);
            lambda = lambda - dLambda;
            if (lambda > 1f) {
                return false;
            }
            if (lambda < 0f) {
                // todo: next check with relative epsilon
                return false;
            }
            if (lambda <= lastLambda) {
                return false;
            // n.setValue(0,0,0);
            // break;
            }
            lastLambda = lambda;
            // interpolate to next lambda
            result.debugDraw(lambda);
            VectorUtil.setInterpolate3(input.transformA, fromA, toA, lambda);
            VectorUtil.setInterpolate3(input.transformB, fromB, toB, lambda);
            gjk.getClosestPoints(input, pointCollector);
            if (pointCollector.hasResult) {
                if (pointCollector.distance < 0f) {
                    result.fraction = lastLambda;
                    n.set(pointCollector.normalOnBInWorld);
                    result.normal.set(n);
                    result.hitPoint.set(pointCollector.pointInWorld);
                    return true;
                }
                c.set(pointCollector.pointInWorld);
                n.set(pointCollector.normalOnBInWorld);
                dist = pointCollector.distance;
            } else {
                // ??
                return false;
            }
        }
        // don't report time of impact for motion away from the contact normal (or causes minor penetration)
        if (n.dot(r) >= -result.allowedPenetration) {
            return false;
        }
        result.fraction = lambda;
        result.normal.set(n);
        result.hitPoint.set(c);
        return true;
    }
    return false;
}

Example 33 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class SubsimplexConvexCast method calcTimeOfImpact.

@Override
public boolean calcTimeOfImpact(Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result) {
    v3 tmp = new v3();
    simplexSolver.reset();
    v3 linVelA = new v3();
    v3 linVelB = new v3();
    linVelA.sub(toA, fromA);
    linVelB.sub(toB, fromB);
    float lambda = 0f;
    Transform interpolatedTransA = new Transform(fromA);
    Transform interpolatedTransB = new Transform(fromB);
    // take relative motion
    v3 r = new v3();
    r.sub(linVelA, linVelB);
    v3 v = new v3();
    tmp.negate(r);
    MatrixUtil.transposeTransform(tmp, tmp, fromA.basis);
    v3 supVertexA = convexA.localGetSupportingVertex(tmp, new v3());
    fromA.transform(supVertexA);
    MatrixUtil.transposeTransform(tmp, r, fromB.basis);
    v3 supVertexB = convexB.localGetSupportingVertex(tmp, new v3());
    fromB.transform(supVertexB);
    v.sub(supVertexA, supVertexB);
    int maxIter = MAX_ITERATIONS;
    v3 n = new v3();
    n.set(0f, 0f, 0f);
    boolean hasResult = false;
    v3 c = new v3();
    float lastLambda = lambda;
    float dist2 = v.lengthSquared();
    // #ifdef BT_USE_DOUBLE_PRECISION
    // btScalar epsilon = btScalar(0.0001);
    // #else
    float epsilon = 0.0001f;
    // #endif
    v3 w = new v3(), p = new v3();
    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) {
            return false;
        }
        if (VdotW > 0f) {
            VdotR = v.dot(r);
            if (VdotR >= -(BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
                return false;
            } else {
                lambda = lambda - VdotW / VdotR;
                // interpolate to next lambda
                // x = s + lambda * r;
                VectorUtil.setInterpolate3(interpolatedTransA, fromA, toA, lambda);
                VectorUtil.setInterpolate3(interpolatedTransB, fromB, toB, 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)
        return false;
    v3 hitA = new v3();
    v3 hitB = new v3();
    simplexSolver.compute_points(hitA, hitB);
    result.hitPoint.set(hitB);
    return true;
}

Example 34 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class GImpactCollisionAlgorithm method gimpact_vs_compoundshape.

public void gimpact_vs_compoundshape(Collidable body0, Collidable body1, GImpactShape shape0, CompoundShape shape1, boolean swapped) {
    Transform orgtrans1 = body1.getWorldTransform(new Transform());
    Transform childtrans1 = new Transform();
    Transform tmpTrans = new Transform();
    int i = shape1.getNumChildShapes();
    while ((i--) != 0) {
        CollisionShape colshape1 = shape1.getChildShape(i);
        childtrans1.mul(orgtrans1, shape1.getChildTransform(i, tmpTrans));
        body1.transform(childtrans1);
        // collide child shape
        gimpact_vs_shape(body0, body1, shape0, colshape1, swapped);
        // restore transforms
        body1.transform(orgtrans1);
    }
}

Example 35 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class GImpactCollisionAlgorithm method gimpact_vs_shape_find_pairs.

protected static void gimpact_vs_shape_find_pairs(Transform trans0, Transform trans1, GImpactShape shape0, CollisionShape shape1, IntArrayList collided_primitives) {
    BoxCollision.AABB boxshape = new BoxCollision.AABB();
    if (shape0.hasBoxSet()) {
        Transform trans1to0 = new Transform();
        trans1to0.inverse(trans0);
        trans1to0.mul(trans1);
        shape1.getAabb(trans1to0, boxshape.min, boxshape.max);
        shape0.getBoxSet().boxQuery(boxshape, collided_primitives);
    } else {
        shape1.getAabb(trans1, boxshape.min, boxshape.max);
        BoxCollision.AABB boxshape0 = new BoxCollision.AABB();
        int i = shape0.getNumChildShapes();
        while ((i--) != 0) {
            shape0.getChildAabb(i, trans0, boxshape0.min, boxshape0.max);
            if (boxshape.has_collision(boxshape0)) {
                collided_primitives.add(i);
            }
        }
    }
}