Examples with Tuple2f spacegraph.util.math.Tuple2f used on opensource projects

Example 51 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

the class AABB method raycast.

/**
 * From Real-time Collision Detection, p179.
 *
 * @param output
 * @param input
 */
public final boolean raycast(final RayCastOutput output, final RayCastInput input, IWorldPool argPool) {
    float tmin = -Float.MAX_VALUE;
    float tmax = Float.MAX_VALUE;
    final Tuple2f p = argPool.popVec2();
    final Tuple2f d = argPool.popVec2();
    final Tuple2f absD = argPool.popVec2();
    final Tuple2f normal = argPool.popVec2();
    p.set(input.p1);
    d.set(input.p2).subbed(input.p1);
    Tuple2f.absToOut(d, absD);
    // x then y
    if (absD.x < Settings.EPSILON) {
        // Parallel.
        if (p.x < lowerBound.x || upperBound.x < p.x) {
            argPool.pushVec2(4);
            return false;
        }
    } else {
        final float inv_d = 1.0f / d.x;
        float t1 = (lowerBound.x - p.x) * inv_d;
        float t2 = (upperBound.x - p.x) * inv_d;
        // Sign of the normal vector.
        float s = -1.0f;
        if (t1 > t2) {
            final float temp = t1;
            t1 = t2;
            t2 = temp;
            s = 1.0f;
        }
        // Push the min up
        if (t1 > tmin) {
            normal.setZero();
            normal.x = s;
            tmin = t1;
        }
        // Pull the max down
        tmax = MathUtils.min(tmax, t2);
        if (tmin > tmax) {
            argPool.pushVec2(4);
            return false;
        }
    }
    if (absD.y < Settings.EPSILON) {
        // Parallel.
        if (p.y < lowerBound.y || upperBound.y < p.y) {
            argPool.pushVec2(4);
            return false;
        }
    } else {
        final float inv_d = 1.0f / d.y;
        float t1 = (lowerBound.y - p.y) * inv_d;
        float t2 = (upperBound.y - p.y) * inv_d;
        // Sign of the normal vector.
        float s = -1.0f;
        if (t1 > t2) {
            final float temp = t1;
            t1 = t2;
            t2 = temp;
            s = 1.0f;
        }
        // Push the min up
        if (t1 > tmin) {
            normal.setZero();
            normal.y = s;
            tmin = t1;
        }
        // Pull the max down
        tmax = MathUtils.min(tmax, t2);
        if (tmin > tmax) {
            argPool.pushVec2(4);
            return false;
        }
    }
    // Does the ray intersect beyond the max fraction?
    if (tmin < 0.0f || input.maxFraction < tmin) {
        argPool.pushVec2(4);
        return false;
    }
    // Intersection.
    output.fraction = tmin;
    output.normal.x = normal.x;
    output.normal.y = normal.y;
    argPool.pushVec2(4);
    return true;
}

Example 52 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

the class DynamicTreeFlatNodes method raycast.

@Override
public void raycast(TreeRayCastCallback callback, RayCastInput input) {
    final Tuple2f p1 = input.p1;
    final Tuple2f p2 = input.p2;
    float p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y;
    float vx, vy;
    float rx, ry;
    float absVx, absVy;
    float cx, cy;
    float hx, hy;
    float tempx, tempy;
    r.x = p2x - p1x;
    r.y = p2y - p1y;
    assert ((r.x * r.x + r.y * r.y) > 0f);
    r.normalize();
    rx = r.x;
    ry = r.y;
    // v is perpendicular to the segment.
    vx = -1f * ry;
    vy = 1f * rx;
    absVx = Math.abs(vx);
    absVy = Math.abs(vy);
    // Separating axis for segment (Gino, p80).
    // |dot(v, p1 - c)| > dot(|v|, h)
    float maxFraction = input.maxFraction;
    // Build a bounding box for the segment.
    final AABB segAABB = aabb;
    // Vec2 t = p1 + maxFraction * (p2 - p1);
    // before inline
    // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
    // Vec2.minToOut(p1, temp, segAABB.lowerBound);
    // Vec2.maxToOut(p1, temp, segAABB.upperBound);
    tempx = (p2x - p1x) * maxFraction + p1x;
    tempy = (p2y - p1y) * maxFraction + p1y;
    segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
    segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
    segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
    segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
    // end inline
    nodeStackIndex = 0;
    nodeStack[nodeStackIndex++] = m_root;
    while (nodeStackIndex > 0) {
        int node = nodeStack[--nodeStackIndex] = m_root;
        if (node == NULL_NODE) {
            continue;
        }
        final AABB nodeAABB = m_aabb[node];
        if (!AABB.testOverlap(nodeAABB, segAABB)) {
            continue;
        }
        // Separating axis for segment (Gino, p80).
        // |dot(v, p1 - c)| > dot(|v|, h)
        // node.aabb.getCenterToOut(c);
        // node.aabb.getExtentsToOut(h);
        cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x) * .5f;
        cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y) * .5f;
        hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x) * .5f;
        hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y) * .5f;
        tempx = p1x - cx;
        tempy = p1y - cy;
        float separation = Math.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy);
        if (separation > 0.0f) {
            continue;
        }
        int child1 = m_child1[node];
        if (child1 == NULL_NODE) {
            subInput.p1.x = p1x;
            subInput.p1.y = p1y;
            subInput.p2.x = p2x;
            subInput.p2.y = p2y;
            subInput.maxFraction = maxFraction;
            float value = callback.raycastCallback(subInput, node);
            if (value == 0.0f) {
                // The client has terminated the ray cast.
                return;
            }
            if (value > 0.0f) {
                // Update segment bounding box.
                maxFraction = value;
                // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
                // Vec2.minToOut(p1, temp, segAABB.lowerBound);
                // Vec2.maxToOut(p1, temp, segAABB.upperBound);
                tempx = (p2x - p1x) * maxFraction + p1x;
                tempy = (p2y - p1y) * maxFraction + p1y;
                segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
                segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
                segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
                segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
            }
        } else {
            nodeStack[nodeStackIndex++] = child1;
            nodeStack[nodeStackIndex++] = m_child2[node];
        }
    }
}

Example 53 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

the class Glass method focee.

@Override
public Tuple2f[] focee(Tuple2f startPoint, Tuple2f vektor) {
    Transform t = new Transform();
    t.set(startPoint, 0);
    int allCount = count * levels;
    Tuple2f[] va = new Tuple2f[allCount];
    for (int l = 0; l < levels; l++) {
        for (int c = 0; c < count; c++) {
            int i = l * count + c;
            // uhol pod ktorym sa nachadza dany bod
            double u = r.nextDouble() * Math.PI * 2;
            double deficit = (r.nextDouble() - 0.5) * m_shattering / 20;
            double r = (l + 1) * m_shattering + deficit;
            double x = Math.sin(u) * r;
            double y = Math.cos(u) * r;
            Tuple2f v = new v2((float) x, (float) y);
            va[i] = Transform.mul(t, v);
        }
    }
    return va;
}

Example 54 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

the class FrictionJoint method solveVelocityConstraints.

@Override
public void solveVelocityConstraints(final SolverData data) {
    v2 vA = data.velocities[m_indexA];
    float wA = data.velocities[m_indexA].w;
    v2 vB = data.velocities[m_indexB];
    float wB = data.velocities[m_indexB].w;
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    float h = data.step.dt;
    // Solve angular friction
    {
        float Cdot = wB - wA;
        float impulse = -m_angularMass * Cdot;
        float oldImpulse = m_angularImpulse;
        float maxImpulse = h * m_maxTorque;
        m_angularImpulse = MathUtils.clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
        impulse = m_angularImpulse - oldImpulse;
        wA -= iA * impulse;
        wB += iB * impulse;
    }
    // Solve linear friction
    {
        final Tuple2f Cdot = pool.popVec2();
        final Tuple2f temp = pool.popVec2();
        Tuple2f.crossToOutUnsafe(wA, m_rA, temp);
        Tuple2f.crossToOutUnsafe(wB, m_rB, Cdot);
        Cdot.added(vB).subbed(vA).subbed(temp);
        final Tuple2f impulse = pool.popVec2();
        Mat22.mulToOutUnsafe(m_linearMass, Cdot, impulse);
        impulse.negated();
        final Tuple2f oldImpulse = pool.popVec2();
        oldImpulse.set(m_linearImpulse);
        m_linearImpulse.added(impulse);
        float maxImpulse = h * m_maxForce;
        if (m_linearImpulse.lengthSquared() > maxImpulse * maxImpulse) {
            m_linearImpulse.normalize();
            m_linearImpulse.scaled(maxImpulse);
        }
        impulse.set(m_linearImpulse).subbed(oldImpulse);
        temp.set(impulse).scaled(mA);
        vA.subbed(temp);
        wA -= iA * Tuple2f.cross(m_rA, impulse);
        temp.set(impulse).scaled(mB);
        vB.added(temp);
        wB += iB * Tuple2f.cross(m_rB, impulse);
    }
    // data.velocities[m_indexA].v.set(vA);
    assert !(data.velocities[m_indexA].w != wA) || (data.velocities[m_indexA].w != wA);
    data.velocities[m_indexA].w = wA;
    // data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;
    pool.pushVec2(4);
}

Example 55 with Tuple2f

use of spacegraph.util.math.Tuple2f in project narchy by automenta.

the class GearJoint method solvePositionConstraints.

@Override
public boolean solvePositionConstraints(SolverData data) {
    Tuple2f cA = data.positions[m_indexA];
    float aA = data.positions[m_indexA].a;
    Tuple2f cB = data.positions[m_indexB];
    float aB = data.positions[m_indexB].a;
    Tuple2f cC = data.positions[m_indexC];
    float aC = data.positions[m_indexC].a;
    Tuple2f cD = data.positions[m_indexD];
    float aD = data.positions[m_indexD].a;
    Rot qA = pool.popRot(), qB = pool.popRot(), qC = pool.popRot(), qD = pool.popRot();
    qA.set(aA);
    qB.set(aB);
    qC.set(aC);
    qD.set(aD);
    float linearError = 0.0f;
    float coordinateA, coordinateB;
    Tuple2f temp = pool.popVec2();
    Tuple2f JvAC = pool.popVec2();
    Tuple2f JvBD = pool.popVec2();
    float JwA, JwB, JwC, JwD;
    float mass = 0.0f;
    if (m_typeA == JointType.REVOLUTE) {
        JvAC.setZero();
        JwA = 1.0f;
        JwC = 1.0f;
        mass += m_iA + m_iC;
        coordinateA = aA - aC - m_referenceAngleA;
    } else {
        Tuple2f rC = pool.popVec2();
        Tuple2f rA = pool.popVec2();
        Tuple2f pC = pool.popVec2();
        Tuple2f pA = pool.popVec2();
        Rot.mulToOutUnsafe(qC, m_localAxisC, JvAC);
        Rot.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subbed(m_lcC), rC);
        Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subbed(m_lcA), rA);
        JwC = Tuple2f.cross(rC, JvAC);
        JwA = Tuple2f.cross(rA, JvAC);
        mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
        pC.set(m_localAnchorC).subbed(m_lcC);
        Rot.mulTransUnsafe(qC, temp.set(rA).added(cA).subbed(cC), pA);
        coordinateA = Tuple2f.dot(pA.subbed(pC), m_localAxisC);
        pool.pushVec2(4);
    }
    if (m_typeB == JointType.REVOLUTE) {
        JvBD.setZero();
        JwB = m_ratio;
        JwD = m_ratio;
        mass += m_ratio * m_ratio * (m_iB + m_iD);
        coordinateB = aB - aD - m_referenceAngleB;
    } else {
        Tuple2f u = pool.popVec2();
        Tuple2f rD = pool.popVec2();
        Tuple2f rB = pool.popVec2();
        Tuple2f pD = pool.popVec2();
        Tuple2f pB = pool.popVec2();
        Rot.mulToOutUnsafe(qD, m_localAxisD, u);
        Rot.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subbed(m_lcD), rD);
        Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subbed(m_lcB), rB);
        JvBD.set(u).scaled(m_ratio);
        JwD = Tuple2f.cross(rD, u);
        JwB = Tuple2f.cross(rB, u);
        mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
        pD.set(m_localAnchorD).subbed(m_lcD);
        Rot.mulTransUnsafe(qD, temp.set(rB).added(cB).subbed(cD), pB);
        coordinateB = Tuple2f.dot(pB.subbed(pD), m_localAxisD);
        pool.pushVec2(5);
    }
    float C = (coordinateA + m_ratio * coordinateB) - m_constant;
    float impulse = 0.0f;
    if (mass > 0.0f) {
        impulse = -C / mass;
    }
    pool.pushVec2(3);
    pool.pushRot(4);
    cA.x += (m_mA * impulse) * JvAC.x;
    cA.y += (m_mA * impulse) * JvAC.y;
    aA += m_iA * impulse * JwA;
    cB.x += (m_mB * impulse) * JvBD.x;
    cB.y += (m_mB * impulse) * JvBD.y;
    aB += m_iB * impulse * JwB;
    cC.x -= (m_mC * impulse) * JvAC.x;
    cC.y -= (m_mC * impulse) * JvAC.y;
    aC -= m_iC * impulse * JwC;
    cD.x -= (m_mD * impulse) * JvBD.x;
    cD.y -= (m_mD * impulse) * JvBD.y;
    aD -= m_iD * impulse * JwD;
    // data.positions[m_indexA].c = cA;
    data.positions[m_indexA].a = aA;
    // data.positions[m_indexB].c = cB;
    data.positions[m_indexB].a = aB;
    // data.positions[m_indexC].c = cC;
    data.positions[m_indexC].a = aC;
    // data.positions[m_indexD].c = cD;
    data.positions[m_indexD].a = aD;
    // TODO_ERIN not implemented
    return linearError < Settings.linearSlop;
}