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

Example 6 with Transform

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

the class GImpactCollisionAlgorithm method gimpact_vs_gimpact.

public void gimpact_vs_gimpact(Collidable body0, Collidable body1, GImpactShape shape0, GImpactShape shape1) {
    if (shape0.getGImpactShapeType() == ShapeType.TRIMESH_SHAPE) {
        GImpactMeshShape meshshape0 = (GImpactMeshShape) shape0;
        int part0 = meshshape0.getMeshPartCount();
        while ((part0--) != 0) {
            gimpact_vs_gimpact(body0, body1, meshshape0.getMeshPart(part0), shape1);
        }
        this.part0 = part0;
        return;
    }
    if (shape1.getGImpactShapeType() == ShapeType.TRIMESH_SHAPE) {
        GImpactMeshShape meshshape1 = (GImpactMeshShape) shape1;
        int part1 = meshshape1.getMeshPartCount();
        while ((part1--) != 0) {
            gimpact_vs_gimpact(body0, body1, shape0, meshshape1.getMeshPart(part1));
        }
        this.part1 = part1;
        return;
    }
    Transform orgtrans0 = body0.getWorldTransform(new Transform());
    Transform orgtrans1 = body1.getWorldTransform(new Transform());
    PairSet pairset = tmpPairset;
    pairset.clear();
    gimpact_vs_gimpact_find_pairs(orgtrans0, orgtrans1, shape0, shape1, pairset);
    if (pairset.size() == 0) {
        return;
    }
    if (shape0.getGImpactShapeType() == ShapeType.TRIMESH_SHAPE_PART && shape1.getGImpactShapeType() == ShapeType.TRIMESH_SHAPE_PART) {
        GImpactMeshShape.GImpactMeshShapePart shapepart0 = (GImpactMeshShape.GImpactMeshShapePart) shape0;
        GImpactMeshShape.GImpactMeshShapePart shapepart1 = (GImpactMeshShape.GImpactMeshShapePart) shape1;
        // specialized function
        // #ifdef BULLET_TRIANGLE_COLLISION
        // collide_gjk_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
        // #else
        collide_sat_triangles(body0, body1, shapepart0, shapepart1, pairset, pairset.size());
        return;
    }
    // general function
    shape0.lockChildShapes();
    shape1.lockChildShapes();
    GIM_ShapeRetriever retriever0 = new GIM_ShapeRetriever(shape0);
    GIM_ShapeRetriever retriever1 = new GIM_ShapeRetriever(shape1);
    boolean child_has_transform0 = shape0.childrenHasTransform();
    boolean child_has_transform1 = shape1.childrenHasTransform();
    Transform tmpTrans = new Transform();
    int i = pairset.size();
    while ((i--) != 0) {
        Pair pair = pairset.get(i);
        triface0 = pair.index1;
        triface1 = pair.index2;
        CollisionShape colshape0 = retriever0.getChildShape(triface0);
        CollisionShape colshape1 = retriever1.getChildShape(triface1);
        if (child_has_transform0) {
            tmpTrans.mul(orgtrans0, shape0.getChildTransform(triface0));
            body0.transform(tmpTrans);
        }
        if (child_has_transform1) {
            tmpTrans.mul(orgtrans1, shape1.getChildTransform(triface1));
            body1.transform(tmpTrans);
        }
        // collide two convex shapes
        convex_vs_convex_collision(body0, body1, colshape0, colshape1);
        if (child_has_transform0) {
            body0.transform(orgtrans0);
        }
        if (child_has_transform1) {
            body1.transform(orgtrans1);
        }
    }
    shape0.unlockChildShapes();
    shape1.unlockChildShapes();
}

Example 7 with Transform

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

the class ConvexConvexAlgorithm method calculateTimeOfImpact.

@Override
public float calculateTimeOfImpact(Collidable col0, Collidable col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    v3 tmp = new v3();
    Transform tmpTrans1 = new Transform();
    Transform tmpTrans2 = new Transform();
    // Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
    // Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
    // col0->m_worldTransform,
    float resultFraction = 1f;
    tmp.sub(col0.getInterpolationWorldTransform(tmpTrans1), col0.getWorldTransform(tmpTrans2));
    if (tmp.lengthSquared() < col0.getCcdSquareMotionThreshold()) {
        tmp.sub(col1.getInterpolationWorldTransform(tmpTrans1), col1.getWorldTransform(tmpTrans2));
        if (tmp.lengthSquared() < col1.getCcdSquareMotionThreshold()) {
            return resultFraction;
        }
    }
    if (disableCcd) {
        return 1f;
    }
    Transform tmpTrans3 = new Transform();
    Transform tmpTrans4 = new Transform();
    // An adhoc way of testing the Continuous Collision Detection algorithms
    // One object is approximated as a sphere, to simplify things
    // Starting in penetration should report no time of impact
    // For proper CCD, better accuracy and handling of 'allowed' penetration should be added
    // also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
    // Convex0 against sphere for Convex1
    {
        ConvexShape convex0 = (ConvexShape) col0.shape();
        // todo: allow non-zero sphere sizes, for better approximation
        SphereShape sphere1 = new SphereShape(col1.getCcdSweptSphereRadius());
        ConvexCast.CastResult result = new ConvexCast.CastResult();
        voronoiSimplex.reset();
        // SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
        // /Simplification, one object is simplified as a sphere
        GjkConvexCast ccd1 = new GjkConvexCast(convex0, sphere1, voronoiSimplex);
        // ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
        if (ccd1.calcTimeOfImpact(col0.getWorldTransform(tmpTrans1), col0.getInterpolationWorldTransform(tmpTrans2), col1.getWorldTransform(tmpTrans3), col1.getInterpolationWorldTransform(tmpTrans4), result)) {
            if (col0.getHitFraction() > result.fraction) {
                col0.setHitFraction(result.fraction);
            }
            if (col1.getHitFraction() > result.fraction) {
                col1.setHitFraction(result.fraction);
            }
            if (resultFraction > result.fraction) {
                resultFraction = result.fraction;
            }
        }
    }
    // Sphere (for convex0) against Convex1
    ConvexShape convex1 = (ConvexShape) col1.shape();
    // todo: allow non-zero sphere sizes, for better approximation
    SphereShape sphere0 = new SphereShape(col0.getCcdSweptSphereRadius());
    ConvexCast.CastResult result = new ConvexCast.CastResult();
    VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
    // SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
    // /Simplification, one object is simplified as a sphere
    GjkConvexCast ccd1 = new GjkConvexCast(sphere0, convex1, voronoiSimplex);
    // ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
    if (ccd1.calcTimeOfImpact(col0.getWorldTransform(tmpTrans1), col0.getInterpolationWorldTransform(tmpTrans2), col1.getWorldTransform(tmpTrans3), col1.getInterpolationWorldTransform(tmpTrans4), result)) {
        if (col0.getHitFraction() > result.fraction) {
            col0.setHitFraction(result.fraction);
        }
        if (col1.getHitFraction() > result.fraction) {
            col1.setHitFraction(result.fraction);
        }
        if (resultFraction > result.fraction) {
            resultFraction = result.fraction;
        }
    }
    return resultFraction;
}

Example 8 with Transform

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

the class Collisions method objectQuerySingle.

/**
 * objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
 */
public static void objectQuerySingle(ConvexShape castShape, Transform convexFromTrans, Transform convexToTrans, Collidable collidable, CollisionShape collisionShape, Transform colObjWorldTransform, ConvexResultCallback resultCallback, float allowedPenetration) {
    if (collisionShape.isConvex()) {
        ConvexCast.CastResult castResult = new ConvexCast.CastResult();
        castResult.allowedPenetration = allowedPenetration;
        // ??
        castResult.fraction = 1f;
        ConvexShape convexShape = (ConvexShape) collisionShape;
        VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
        GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = new GjkEpaPenetrationDepthSolver();
        // JAVA TODO: should be convexCaster1
        // ContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
        GjkConvexCast convexCaster2 = new GjkConvexCast(castShape, convexShape, simplexSolver);
        // btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
        ConvexCast castPtr = convexCaster2;
        if (castPtr.calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) {
            // add hit
            if (castResult.normal.lengthSquared() > 0.0001f) {
                if (castResult.fraction < resultCallback.closestHitFraction) {
                    castResult.normal.normalize();
                    LocalConvexResult localConvexResult = new LocalConvexResult(collidable, null, castResult.normal, castResult.hitPoint, castResult.fraction);
                    boolean normalInWorldSpace = true;
                    resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
                }
            }
        }
    } else {
        if (collisionShape.isConcave()) {
            if (collisionShape.getShapeType() == BroadphaseNativeType.TRIANGLE_MESH_SHAPE_PROXYTYPE) {
                BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape) collisionShape;
                Transform worldTocollisionObject = new Transform();
                worldTocollisionObject.inverse(colObjWorldTransform);
                v3 convexFromLocal = new v3();
                convexFromLocal.set(convexFromTrans);
                worldTocollisionObject.transform(convexFromLocal);
                v3 convexToLocal = new v3();
                convexToLocal.set(convexToTrans);
                worldTocollisionObject.transform(convexToLocal);
                // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
                Transform rotationXform = new Transform();
                Matrix3f tmpMat = new Matrix3f();
                tmpMat.mul(worldTocollisionObject.basis, convexToTrans.basis);
                rotationXform.set(tmpMat);
                BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, convexFromTrans, convexToTrans, resultCallback, collidable, triangleMesh, colObjWorldTransform);
                tccb.hitFraction = resultCallback.closestHitFraction;
                tccb.normalInWorldSpace = true;
                v3 boxMinLocal = new v3();
                v3 boxMaxLocal = new v3();
                castShape.getAabb(rotationXform, boxMinLocal, boxMaxLocal);
                triangleMesh.performConvexcast(tccb, convexFromLocal, convexToLocal, boxMinLocal, boxMaxLocal);
            } else {
                ConcaveShape triangleMesh = (ConcaveShape) collisionShape;
                Transform worldTocollisionObject = new Transform();
                worldTocollisionObject.inverse(colObjWorldTransform);
                v3 convexFromLocal = new v3();
                convexFromLocal.set(convexFromTrans);
                worldTocollisionObject.transform(convexFromLocal);
                v3 convexToLocal = new v3();
                convexToLocal.set(convexToTrans);
                worldTocollisionObject.transform(convexToLocal);
                // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
                Transform rotationXform = new Transform();
                Matrix3f tmpMat = new Matrix3f();
                tmpMat.mul(worldTocollisionObject.basis, convexToTrans.basis);
                rotationXform.set(tmpMat);
                BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, convexFromTrans, convexToTrans, resultCallback, collidable, triangleMesh, colObjWorldTransform);
                tccb.hitFraction = resultCallback.closestHitFraction;
                tccb.normalInWorldSpace = false;
                v3 boxMinLocal = new v3();
                v3 boxMaxLocal = new v3();
                castShape.getAabb(rotationXform, boxMinLocal, boxMaxLocal);
                v3 rayAabbMinLocal = new v3(convexFromLocal);
                VectorUtil.setMin(rayAabbMinLocal, convexToLocal);
                v3 rayAabbMaxLocal = new v3(convexFromLocal);
                VectorUtil.setMax(rayAabbMaxLocal, convexToLocal);
                rayAabbMinLocal.add(boxMinLocal);
                rayAabbMaxLocal.add(boxMaxLocal);
                triangleMesh.processAllTriangles(tccb, rayAabbMinLocal, rayAabbMaxLocal);
            }
        } else {
            // todo: use AABB tree or other BVH acceleration structure!
            if (collisionShape.isCompound()) {
                CompoundShape compoundShape = (CompoundShape) collisionShape;
                for (int i = 0; i < compoundShape.getNumChildShapes(); i++) {
                    Transform childTrans = compoundShape.getChildTransform(i, new Transform());
                    CollisionShape childCollisionShape = compoundShape.getChildShape(i);
                    Transform childWorldTrans = new Transform();
                    childWorldTrans.mul(colObjWorldTransform, childTrans);
                    // replace collision shape so that callback can determine the triangle
                    CollisionShape saveCollisionShape = collidable.shape();
                    collidable.internalSetTemporaryCollisionShape(childCollisionShape);
                    objectQuerySingle(castShape, convexFromTrans, convexToTrans, collidable, childCollisionShape, childWorldTrans, resultCallback, allowedPenetration);
                    // restore
                    collidable.internalSetTemporaryCollisionShape(saveCollisionShape);
                }
            }
        }
    }
}

Example 9 with Transform

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

the class Collisions method updateSingleAabb.

// JAVA NOTE: ported from 2.74, missing contact threshold stuff
protected void updateSingleAabb(Collidable colObj) {
    v3 minAabb = new v3(), maxAabb = new v3();
    v3 tmp = new v3();
    Transform tmpTrans = new Transform();
    colObj.shape().getAabb(colObj.getWorldTransform(tmpTrans), minAabb, maxAabb);
    // need to increase the aabb for contact thresholds
    v3 contactThreshold = new v3();
    float bt = getContactBreakingThreshold();
    contactThreshold.set(bt, bt, bt);
    minAabb.sub(contactThreshold);
    maxAabb.add(contactThreshold);
    Broadphase bp = broadphase;
    // moving objects should be moderately sized, probably something wrong if not
    // TODO: optimize
    tmp.sub(maxAabb, minAabb);
    if (colObj.isStaticObject() || (tmp.lengthSquared() < maxAABBLength)) {
        Broadphasing broadphase = colObj.broadphase;
        if (broadphase == null)
            throw new RuntimeException();
        bp.setAabb(broadphase, minAabb, maxAabb, intersecter);
    } else {
        // something went wrong, investigate
        // this assert is unwanted in 3D modelers (danger of loosing work)
        colObj.setActivationState(Collidable.DISABLE_SIMULATION);
    // if (updateAabbs_reportMe && debugDrawer != null) {
    // updateAabbs_reportMe = false;
    // debugDrawer.reportErrorWarning("Overflow in AABB, object removed from simulation");
    // debugDrawer.reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
    // debugDrawer.reportErrorWarning("Please include above information, your Platform, version of OS.\n");
    // debugDrawer.reportErrorWarning("Thanks.\n");
    // }
    }
}

Example 10 with Transform

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

the class ConeTwistConstraint method buildJacobian.

@Override
public void buildJacobian() {
    v3 tmp = new v3();
    v3 tmp1 = new v3();
    v3 tmp2 = new v3();
    Transform tmpTrans = new Transform();
    appliedImpulse = 0f;
    // set bias, sign, clear accumulator
    swingCorrection = 0f;
    twistLimitSign = 0f;
    solveTwistLimit = false;
    solveSwingLimit = false;
    accTwistLimitImpulse = 0f;
    accSwingLimitImpulse = 0f;
    if (!angularOnly) {
        v3 pivotAInW = new v3(rbAFrame);
        rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);
        v3 pivotBInW = new v3(rbBFrame);
        rbB.getCenterOfMassTransform(tmpTrans).transform(pivotBInW);
        v3 relPos = new v3();
        relPos.sub(pivotBInW, pivotAInW);
        // TODO: stack
        v3[] normal = /*[3]*/
        { new v3(), new v3(), new v3() };
        if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) {
            normal[0].normalize(relPos);
        } else {
            normal[0].set(1f, 0f, 0f);
        }
        TransformUtil.planeSpace1(normal[0], normal[1], normal[2]);
        for (int i = 0; i < 3; i++) {
            Matrix3f mat1 = rbA.getCenterOfMassTransform(new Transform()).basis;
            mat1.transpose();
            Matrix3f mat2 = rbB.getCenterOfMassTransform(new Transform()).basis;
            mat2.transpose();
            tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmp));
            tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmp));
            jac[i].init(mat1, mat2, tmp1, tmp2, normal[i], rbA.getInvInertiaDiagLocal(new v3()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(new v3()), rbB.getInvMass());
        }
    }
    v3 b1Axis1 = new v3(), b1Axis2 = new v3(), b1Axis3 = new v3();
    v3 b2Axis1 = new v3(), b2Axis2 = new v3();
    rbAFrame.basis.getColumn(0, b1Axis1);
    getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis1);
    rbBFrame.basis.getColumn(0, b2Axis1);
    getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis1);
    float swing1 = 0f, swing2 = 0f;
    float swx = 0f, swy = 0f;
    float thresh = 10f;
    float fact;
    // Get Frame into world space
    if (swingSpan1 >= 0.05f) {
        rbAFrame.basis.getColumn(1, b1Axis2);
        getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis2);
        // swing1 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis2), b2Axis1.dot(b1Axis1));
        swx = b2Axis1.dot(b1Axis1);
        swy = b2Axis1.dot(b1Axis2);
        swing1 = ScalarUtil.atan2Fast(swy, swx);
        fact = (swy * swy + swx * swx) * thresh * thresh;
        fact = fact / (fact + 1f);
        swing1 *= fact;
    }
    if (swingSpan2 >= 0.05f) {
        rbAFrame.basis.getColumn(2, b1Axis3);
        getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis3);
        // swing2 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis3), b2Axis1.dot(b1Axis1));
        swx = b2Axis1.dot(b1Axis1);
        swy = b2Axis1.dot(b1Axis3);
        swing2 = ScalarUtil.atan2Fast(swy, swx);
        fact = (swy * swy + swx * swx) * thresh * thresh;
        fact = fact / (fact + 1f);
        swing2 *= fact;
    }
    float RMaxAngle1Sq = 1.0f / (swingSpan1 * swingSpan1);
    float RMaxAngle2Sq = 1.0f / (swingSpan2 * swingSpan2);
    float EllipseAngle = Math.abs(swing1 * swing1) * RMaxAngle1Sq + Math.abs(swing2 * swing2) * RMaxAngle2Sq;
    if (EllipseAngle > 1.0f) {
        swingCorrection = EllipseAngle - 1.0f;
        solveSwingLimit = true;
        // Calculate necessary axis & factors
        tmp1.scale(b2Axis1.dot(b1Axis2), b1Axis2);
        tmp2.scale(b2Axis1.dot(b1Axis3), b1Axis3);
        tmp.add(tmp1, tmp2);
        swingAxis.cross(b2Axis1, tmp);
        swingAxis.normalize();
        float swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
        swingAxis.scale(swingAxisSign);
        kSwing = 1f / (getRigidBodyA().computeAngularImpulseDenominator(swingAxis) + getRigidBodyB().computeAngularImpulseDenominator(swingAxis));
    }
    // Twist limits
    if (twistSpan >= 0f) {
        // Vector3f b2Axis2 = new Vector3f();
        rbBFrame.basis.getColumn(1, b2Axis2);
        getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis2);
        Quat4f rotationArc = QuaternionUtil.shortestArcQuat(b2Axis1, b1Axis1, new Quat4f());
        v3 TwistRef = QuaternionUtil.quatRotate(rotationArc, b2Axis2, new v3());
        float twist = ScalarUtil.atan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
        float lockedFreeFactor = (twistSpan > 0.05f) ? limitSoftness : 0f;
        if (twist <= -twistSpan * lockedFreeFactor) {
            twistCorrection = -(twist + twistSpan);
            solveTwistLimit = true;
            twistAxis.add(b2Axis1, b1Axis1);
            twistAxis.scale(0.5f);
            twistAxis.normalize();
            twistAxis.scale(-1.0f);
            kTwist = 1f / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis) + getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
        } else if (twist > twistSpan * lockedFreeFactor) {
            twistCorrection = (twist - twistSpan);
            solveTwistLimit = true;
            twistAxis.add(b2Axis1, b1Axis1);
            twistAxis.scale(0.5f);
            twistAxis.normalize();
            kTwist = 1f / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis) + getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
        }
    }
}