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

Example 46 with Tuple2f

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

the class PositionSolverManifold method solveTOIPositionConstraints.

// Sequential position solver for position constraints.
public boolean solveTOIPositionConstraints(int toiIndexA, int toiIndexB) {
    float minSeparation = 0.0f;
    for (int i = 0; i < m_count; ++i) {
        ContactPositionConstraint pc = m_positionConstraints[i];
        int indexA = pc.indexA;
        int indexB = pc.indexB;
        Tuple2f localCenterA = pc.localCenterA;
        Tuple2f localCenterB = pc.localCenterB;
        final float localCenterAx = localCenterA.x;
        final float localCenterAy = localCenterA.y;
        final float localCenterBx = localCenterB.x;
        final float localCenterBy = localCenterB.y;
        int pointCount = pc.pointCount;
        float mA = 0.0f;
        float iA = 0.0f;
        if (indexA == toiIndexA || indexA == toiIndexB) {
            mA = pc.invMassA;
            iA = pc.invIA;
        }
        float mB = 0.0f;
        float iB = 0.0f;
        if (indexB == toiIndexA || indexB == toiIndexB) {
            mB = pc.invMassB;
            iB = pc.invIB;
        }
        Tuple2f cA = m_positions[indexA];
        float aA = m_positions[indexA].a;
        Tuple2f cB = m_positions[indexB];
        float aB = m_positions[indexB].a;
        // Solve normal constraints
        for (int j = 0; j < pointCount; ++j) {
            final Rot xfAq = xfA;
            final Rot xfBq = xfB;
            xfAq.set(aA);
            xfBq.set(aB);
            xfA.pos.x = cA.x - xfAq.c * localCenterAx + xfAq.s * localCenterAy;
            xfA.pos.y = cA.y - xfAq.s * localCenterAx - xfAq.c * localCenterAy;
            xfB.pos.x = cB.x - xfBq.c * localCenterBx + xfBq.s * localCenterBy;
            xfB.pos.y = cB.y - xfBq.s * localCenterBx - xfBq.c * localCenterBy;
            final PositionSolverManifold psm = psolver;
            psm.initialize(pc, xfA, xfB, j);
            Tuple2f normal = psm.normal;
            Tuple2f point = psm.point;
            float separation = psm.separation;
            float rAx = point.x - cA.x;
            float rAy = point.y - cA.y;
            float rBx = point.x - cB.x;
            float rBy = point.y - cB.y;
            // Track max constraint error.
            minSeparation = MathUtils.min(minSeparation, separation);
            // Prevent large corrections and allow slop.
            float C = MathUtils.clamp(Settings.toiBaugarte * (separation + Settings.linearSlop), -Settings.maxLinearCorrection, 0.0f);
            // Compute the effective mass.
            float rnA = rAx * normal.y - rAy * normal.x;
            float rnB = rBx * normal.y - rBy * normal.x;
            float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
            // Compute normal impulse
            float impulse = K > 0.0f ? -C / K : 0.0f;
            float Px = normal.x * impulse;
            float Py = normal.y * impulse;
            cA.x -= Px * mA;
            cA.y -= Py * mA;
            aA -= iA * (rAx * Py - rAy * Px);
            cB.x += Px * mB;
            cB.y += Py * mB;
            aB += iB * (rBx * Py - rBy * Px);
        }
        // m_positions[indexA].c.set(cA);
        m_positions[indexA].a = aA;
        // m_positions[indexB].c.set(cB);
        m_positions[indexB].a = aB;
    }
    // push the separation above -_linearSlop.
    return minSeparation >= -1.5f * Settings.linearSlop;
}

Example 47 with Tuple2f

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

the class PositionSolverManifold method solvePositionConstraints.

/**
 * Sequential solver.
 */
public final boolean solvePositionConstraints() {
    float minSeparation = 0.0f;
    for (int i = 0; i < m_count; ++i) {
        ContactPositionConstraint pc = m_positionConstraints[i];
        int indexA = pc.indexA;
        int indexB = pc.indexB;
        float mA = pc.invMassA;
        float iA = pc.invIA;
        Tuple2f localCenterA = pc.localCenterA;
        final float localCenterAx = localCenterA.x;
        final float localCenterAy = localCenterA.y;
        float mB = pc.invMassB;
        float iB = pc.invIB;
        Tuple2f localCenterB = pc.localCenterB;
        final float localCenterBx = localCenterB.x;
        final float localCenterBy = localCenterB.y;
        int pointCount = pc.pointCount;
        Tuple2f cA = m_positions[indexA];
        float aA = m_positions[indexA].a;
        Tuple2f cB = m_positions[indexB];
        float aB = m_positions[indexB].a;
        // Solve normal constraints
        for (int j = 0; j < pointCount; ++j) {
            final Rot xfAq = xfA;
            final Rot xfBq = xfB;
            xfAq.set(aA);
            xfBq.set(aB);
            xfA.pos.x = cA.x - xfAq.c * localCenterAx + xfAq.s * localCenterAy;
            xfA.pos.y = cA.y - xfAq.s * localCenterAx - xfAq.c * localCenterAy;
            xfB.pos.x = cB.x - xfBq.c * localCenterBx + xfBq.s * localCenterBy;
            xfB.pos.y = cB.y - xfBq.s * localCenterBx - xfBq.c * localCenterBy;
            final PositionSolverManifold psm = psolver;
            psm.initialize(pc, xfA, xfB, j);
            final Tuple2f normal = psm.normal;
            final Tuple2f point = psm.point;
            final float separation = psm.separation;
            float rAx = point.x - cA.x;
            float rAy = point.y - cA.y;
            float rBx = point.x - cB.x;
            float rBy = point.y - cB.y;
            // Track max constraint error.
            minSeparation = MathUtils.min(minSeparation, separation);
            // Prevent large corrections and allow slop.
            final float C = MathUtils.clamp(Settings.baumgarte * (separation + Settings.linearSlop), -Settings.maxLinearCorrection, 0.0f);
            // Compute the effective mass.
            final float rnA = rAx * normal.y - rAy * normal.x;
            final float rnB = rBx * normal.y - rBy * normal.x;
            final float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
            // Compute normal impulse
            final float impulse = K > 0.0f ? -C / K : 0.0f;
            float Px = normal.x * impulse;
            float Py = normal.y * impulse;
            cA.x -= Px * mA;
            cA.y -= Py * mA;
            aA -= iA * (rAx * Py - rAy * Px);
            cB.x += Px * mB;
            cB.y += Py * mB;
            aB += iB * (rBx * Py - rBy * Px);
        }
        // m_positions[indexA].c.set(cA);
        m_positions[indexA].a = aA;
        // m_positions[indexB].c.set(cB);
        m_positions[indexB].a = aB;
    }
    // push the separation above -linearSlop.
    return minSeparation >= -3.0f * Settings.linearSlop;
}

Example 48 with Tuple2f

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

the class ConstantVolumeJoint method constrainEdges.

private boolean constrainEdges(Position[] positions) {
    float perimeter = 0.0f;
    for (int i = 0; i < bodies.length; ++i) {
        final int next = (i == bodies.length - 1) ? 0 : i + 1;
        float dx = positions[bodies[next].island].x - positions[bodies[i].island].x;
        float dy = positions[bodies[next].island].y - positions[bodies[i].island].y;
        float dist = (float) Math.sqrt(dx * dx + dy * dy);
        if (dist < Settings.EPSILON) {
            dist = 1.0f;
        }
        normals[i].x = dy / dist;
        normals[i].y = -dx / dist;
        perimeter += dist;
    }
    final Tuple2f delta = pool.popVec2();
    float deltaArea = targetVolume - getSolverArea(positions);
    // *relaxationFactor
    float toExtrude = 0.5f * deltaArea / perimeter;
    // float sumdeltax = 0.0f;
    boolean done = true;
    for (int i = 0; i < bodies.length; ++i) {
        final int next = (i == bodies.length - 1) ? 0 : i + 1;
        delta.set(toExtrude * (normals[i].x + normals[next].x), toExtrude * (normals[i].y + normals[next].y));
        // sumdeltax += dx;
        float normSqrd = delta.lengthSquared();
        if (normSqrd > Settings.maxLinearCorrection * Settings.maxLinearCorrection) {
            delta.scaled(Settings.maxLinearCorrection / (float) Math.sqrt(normSqrd));
        }
        if (normSqrd > Settings.linearSlop * Settings.linearSlop) {
            done = false;
        }
        positions[bodies[next].island].x += delta.x;
        positions[bodies[next].island].y += delta.y;
    // bodies[next].m_linearVelocity.x += delta.x * step.inv_dt;
    // bodies[next].m_linearVelocity.y += delta.y * step.inv_dt;
    }
    pool.pushVec2(1);
    // System.out.println(sumdeltax);
    return done;
}

Example 49 with Tuple2f

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

the class DistanceJoint method solvePositionConstraints.

@Override
public boolean solvePositionConstraints(final SolverData data) {
    if (m_frequencyHz > 0.0f) {
        return true;
    }
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Tuple2f rA = pool.popVec2();
    final Tuple2f rB = pool.popVec2();
    final Tuple2f u = pool.popVec2();
    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;
    qA.set(aA);
    qB.set(aB);
    Rot.mulToOutUnsafe(qA, u.set(m_localAnchorA).subbed(m_localCenterA), rA);
    Rot.mulToOutUnsafe(qB, u.set(m_localAnchorB).subbed(m_localCenterB), rB);
    u.set(cB).added(rB).subbed(cA).subbed(rA);
    float length = u.normalize();
    float C = length - m_length;
    C = MathUtils.clamp(C, -Settings.maxLinearCorrection, Settings.maxLinearCorrection);
    float impulse = -m_mass * C;
    float Px = impulse * u.x;
    float Py = impulse * u.y;
    cA.x -= m_invMassA * Px;
    cA.y -= m_invMassA * Py;
    aA -= m_invIA * (rA.x * Py - rA.y * Px);
    cB.x += m_invMassB * Px;
    cB.y += m_invMassB * Py;
    aB += m_invIB * (rB.x * Py - rB.y * Px);
    // data.positions[m_indexA].c.set(cA);
    data.positions[m_indexA].a = aA;
    // data.positions[m_indexB].c.set(cB);
    data.positions[m_indexB].a = aB;
    pool.pushVec2(3);
    pool.pushRot(2);
    return Math.abs(C) < Settings.linearSlop;
}

Example 50 with Tuple2f

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

the class DistanceJoint method initVelocityConstraints.

@Override
public void initVelocityConstraints(final SolverData data) {
    m_indexA = A.island;
    m_indexB = B.island;
    m_localCenterA.set(A.sweep.localCenter);
    m_localCenterB.set(B.sweep.localCenter);
    m_invMassA = A.m_invMass;
    m_invMassB = B.m_invMass;
    m_invIA = A.m_invI;
    m_invIB = B.m_invI;
    Tuple2f cA = data.positions[m_indexA];
    float aA = data.positions[m_indexA].a;
    Tuple2f vA = data.velocities[m_indexA];
    float wA = data.velocities[m_indexA].w;
    Tuple2f cB = data.positions[m_indexB];
    float aB = data.positions[m_indexB].a;
    Tuple2f vB = data.velocities[m_indexB];
    float wB = data.velocities[m_indexB].w;
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    qA.set(aA);
    qB.set(aB);
    // use m_u as temporary variable
    Rot.mulToOutUnsafe(qA, m_u.set(m_localAnchorA).subbed(m_localCenterA), m_rA);
    Rot.mulToOutUnsafe(qB, m_u.set(m_localAnchorB).subbed(m_localCenterB), m_rB);
    m_u.set(cB).added(m_rB).subbed(cA).subbed(m_rA);
    pool.pushRot(2);
    // Handle singularity.
    float length = m_u.length();
    if (length > Settings.linearSlop) {
        m_u.x *= 1.0f / length;
        m_u.y *= 1.0f / length;
    } else {
        m_u.set(0.0f, 0.0f);
    }
    float crAu = Tuple2f.cross(m_rA, m_u);
    float crBu = Tuple2f.cross(m_rB, m_u);
    float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;
    // Compute the effective mass matrix.
    m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
    if (m_frequencyHz > 0.0f) {
        float C = length - m_length;
        // Frequency
        float omega = 2.0f * MathUtils.PI * m_frequencyHz;
        // Damping coefficient
        float d = 2.0f * m_mass * m_dampingRatio * omega;
        // Spring stiffness
        float k = m_mass * omega * omega;
        // magic formulas
        float h = data.step.dt;
        m_gamma = h * (d + h * k);
        m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
        m_bias = C * h * k * m_gamma;
        invMass += m_gamma;
        m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
    } else {
        m_gamma = 0.0f;
        m_bias = 0.0f;
    }
    if (data.step.warmStarting) {
        // Scale the impulse to support a variable time step.
        m_impulse *= data.step.dtRatio;
        Tuple2f P = pool.popVec2();
        P.set(m_u).scaled(m_impulse);
        vA.x -= m_invMassA * P.x;
        vA.y -= m_invMassA * P.y;
        wA -= m_invIA * Tuple2f.cross(m_rA, P);
        vB.x += m_invMassB * P.x;
        vB.y += m_invMassB * P.y;
        wB += m_invIB * Tuple2f.cross(m_rB, P);
        pool.pushVec2(1);
    } else {
        m_impulse = 0.0f;
    }
    // data.velocities[m_indexA].v.set(vA);
    data.velocities[m_indexA].w = wA;
    // data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;
}