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

Example 26 with Tuple2f

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

the class Mat22 method solve.

/**
 * Solve A * x = b where A = this matrix.
 *
 * @return The vector x that solves the above equation.
 */
public final Tuple2f solve(final Tuple2f b) {
    final float a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
    float det = a11 * a22 - a12 * a21;
    if (det != 0.0f) {
        det = 1.0f / det;
    }
    final Tuple2f x = new v2(det * (a22 * b.x - a12 * b.y), det * (a11 * b.y - a21 * b.x));
    return x;
}

Example 27 with Tuple2f

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

the class Island method solveTOI.

void solveTOI(TimeStep subStep, int toiIndexA, int toiIndexB) {
    assert (toiIndexA < m_bodyCount);
    assert (toiIndexB < m_bodyCount);
    // Initialize the body state.
    for (int i = 0; i < m_bodyCount; ++i) {
        Body2D b = bodies[i];
        positions[i].x = b.sweep.c.x;
        positions[i].y = b.sweep.c.y;
        positions[i].a = b.sweep.a;
        velocities[i].x = b.vel.x;
        velocities[i].y = b.vel.y;
        velocities[i].w = b.velAngular;
    }
    toiSolverDef.contacts = contacts;
    toiSolverDef.count = m_contactCount;
    toiSolverDef.step = subStep;
    toiSolverDef.positions = positions;
    toiSolverDef.velocities = velocities;
    toiContactSolver.init(toiSolverDef);
    // Solve position constraints.
    for (int i = 0; i < subStep.positionIterations; ++i) {
        boolean contactsOkay = toiContactSolver.solveTOIPositionConstraints(toiIndexA, toiIndexB);
        if (contactsOkay) {
            break;
        }
    }
    // #if 0
    // // Is the new position really safe?
    // for (int i = 0; i < m_contactCount; ++i)
    // {
    // Contact* c = m_contacts[i];
    // Fixture* fA = c.GetFixtureA();
    // Fixture* fB = c.GetFixtureB();
    // 
    // Body bA = fA.GetBody();
    // Body bB = fB.GetBody();
    // 
    // int indexA = c.GetChildIndexA();
    // int indexB = c.GetChildIndexB();
    // 
    // DistanceInput input;
    // input.proxyA.Set(fA.GetShape(), indexA);
    // input.proxyB.Set(fB.GetShape(), indexB);
    // input.transformA = bA.GetTransform();
    // input.transformB = bB.GetTransform();
    // input.useRadii = false;
    // 
    // DistanceOutput output;
    // SimplexCache cache;
    // cache.count = 0;
    // Distance(&output, &cache, &input);
    // 
    // if (output.distance == 0 || cache.count == 3)
    // {
    // cache.count += 0;
    // }
    // }
    // #endif
    // Leap of faith to new safe state.
    bodies[toiIndexA].sweep.c0.x = positions[toiIndexA].x;
    bodies[toiIndexA].sweep.c0.y = positions[toiIndexA].y;
    bodies[toiIndexA].sweep.a0 = positions[toiIndexA].a;
    bodies[toiIndexB].sweep.c0.set(positions[toiIndexB]);
    bodies[toiIndexB].sweep.a0 = positions[toiIndexB].a;
    // No warm starting is needed for TOI events because warm
    // starting impulses were applied in the discrete solver.
    toiContactSolver.initializeVelocityConstraints();
    // Solve velocity constraints.
    for (int i = 0; i < subStep.velocityIterations; ++i) {
        toiContactSolver.solveVelocityConstraints();
    }
    // Don't store the TOI contact forces for warm starting
    // because they can be quite large.
    float h = subStep.dt;
    // Integrate positions
    for (int i = 0; i < m_bodyCount; ++i) {
        Tuple2f c = positions[i];
        float a = positions[i].a;
        Tuple2f v = velocities[i];
        float w = velocities[i].w;
        // Check for large velocities
        float translationx = v.x * h;
        float translationy = v.y * h;
        if (translationx * translationx + translationy * translationy > Settings.maxTranslationSquared) {
            float ratio = Settings.maxTranslation / (float) Math.sqrt(translationx * translationx + translationy * translationy);
            v.scaled(ratio);
        }
        float rotation = h * w;
        if (rotation * rotation > Settings.maxRotationSquared) {
            float ratio = Settings.maxRotation / Math.abs(rotation);
            w *= ratio;
        }
        // Integrate
        c.x += v.x * h;
        c.y += v.y * h;
        a += h * w;
        positions[i].x = c.x;
        positions[i].y = c.y;
        positions[i].a = a;
        velocities[i].x = v.x;
        velocities[i].y = v.y;
        velocities[i].w = w;
        // Sync bodies
        Body2D body = bodies[i];
        body.sweep.c.x = c.x;
        body.sweep.c.y = c.y;
        body.sweep.a = a;
        body.vel.x = v.x;
        body.vel.y = v.y;
        body.velAngular = w;
        body.synchronizeTransform();
    }
    report(toiContactSolver.m_velocityConstraints);
}

Example 28 with Tuple2f

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

the class Triangle method init.

/**
 * Inicializuje trojuholnik
 *
 * @param i 1. index vrcholu
 * @param j 2. index vrcholu
 * @param k 3. index vrcholu
 * @param p Pole vrcholov
 * @param t protilahly trojuholnik
 */
final void init(final int i, final int j, final int k, final Tuple2f[] p, final Triangle t) {
    this.i = i;
    this.j = j;
    this.k = k;
    // nastavi stred opisanej kruznice
    Tuple2f a = p[i];
    Tuple2f b = p[j];
    Tuple2f c = p[k];
    double Bx = (double) b.x - a.x;
    double By = (double) b.y - a.y;
    double Cx = (double) c.x - a.x;
    double Cy = (double) c.y - a.y;
    // overene - padne to len ak su body uuuuplne identicke, ak niesu, Bx | By a Cx | Cy != 0 a D musi byt rozumne cislo
    double D = 1.0 / (2 * (Bx * Cy - By * Cx));
    double Bs = Bx * Bx + By * By;
    double Cs = Cx * Cx + Cy * Cy;
    double x = (Cy * Bs - By * Cs) * D;
    double y = (Bx * Cs - Cx * Bs) * D;
    dX = x + a.x;
    dY = y + a.y;
    // nastavi polomer^2 opisanej kruznice
    // polomer treba trochu znizit kvoli probleme zaokruhlovania (aritmetika ho defaultne robi o epsilon vacsi, ako je treba, co sposobuje pri extremnych vstupoch exception)
    r = (x * x + y * y) * (1.0 - 1E-15);
    // nastavi ohisko, pokial sa zhoduje suradnicami s ohniskom protilahleho trojuholnika, tak pouzije jeho index
    if (t != null && (float) dX == (float) t.dX && (float) dY == (float) t.dY) {
        // podmienka zarucuje, ze focusCorelation vzdy vedie ku korenovemu trianglu
        focusCorelation = t.focusCorelation != null ? t.focusCorelation : t;
    } else {
        focusCorelation = null;
    }
}

Example 29 with Tuple2f

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

the class ParticleSystem method solveRigid.

void solveRigid(final TimeStep step) {
    for (ParticleGroup group = m_groupList; group != null; group = group.getNext()) {
        if ((group.m_groupFlags & ParticleGroupType.b2_rigidParticleGroup) != 0) {
            group.updateStatistics();
            Tuple2f temp = tempVec;
            Tuple2f cross = tempv2;
            Rot rotation = tempRot;
            rotation.set(step.dt * group.m_angularVelocity);
            Rot.mulToOutUnsafe(rotation, group.m_center, cross);
            temp.set(group.m_linearVelocity).scaled(step.dt).added(group.m_center).subbed(cross);
            tempXf.pos.set(temp);
            tempXf.set(rotation);
            Transform.mulToOut(tempXf, group.m_transform, group.m_transform);
            final Transform velocityTransform = tempXf2;
            velocityTransform.pos.x = step.inv_dt * tempXf.pos.x;
            velocityTransform.pos.y = step.inv_dt * tempXf.pos.y;
            velocityTransform.s = step.inv_dt * tempXf.s;
            velocityTransform.c = step.inv_dt * (tempXf.c - 1);
            for (int i = group.m_firstIndex; i < group.m_lastIndex; i++) {
                Transform.mulToOutUnsafe(velocityTransform, m_positionBuffer.data[i], m_velocityBuffer.data[i]);
            }
        }
    }
}

Example 30 with Tuple2f

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

the class ParticleSystem method solve.

public void solve(TimeStep step) {
    ++m_timestamp;
    if (m_count == 0) {
        return;
    }
    m_allParticleFlags = 0;
    for (int i = 0; i < m_count; i++) {
        m_allParticleFlags |= m_flagsBuffer.data[i];
    }
    if ((m_allParticleFlags & ParticleType.b2_zombieParticle) != 0) {
        solveZombie();
    }
    if (m_count == 0) {
        return;
    }
    m_allGroupFlags = 0;
    for (ParticleGroup group = m_groupList; group != null; group = group.getNext()) {
        m_allGroupFlags |= group.m_groupFlags;
    }
    final float gravityx = step.dt * m_gravityScale * m_world.getGravity().x;
    final float gravityy = step.dt * m_gravityScale * m_world.getGravity().y;
    float criticalVelocytySquared = getCriticalVelocitySquared(step);
    for (int i = 0; i < m_count; i++) {
        Tuple2f v = m_velocityBuffer.data[i];
        v.x += gravityx;
        v.y += gravityy;
        float v2 = v.x * v.x + v.y * v.y;
        if (v2 > criticalVelocytySquared) {
            float a = v2 == 0 ? Float.MAX_VALUE : (float) Math.sqrt(criticalVelocytySquared / v2);
            v.x *= a;
            v.y *= a;
        }
    }
    solveCollision(step);
    if ((m_allGroupFlags & ParticleGroupType.b2_rigidParticleGroup) != 0) {
        solveRigid(step);
    }
    if ((m_allParticleFlags & ParticleType.b2_wallParticle) != 0) {
        solveWall(step);
    }
    for (int i = 0; i < m_count; i++) {
        Tuple2f pos = m_positionBuffer.data[i];
        Tuple2f vel = m_velocityBuffer.data[i];
        pos.x += step.dt * vel.x;
        pos.y += step.dt * vel.y;
    }
    updateBodyContacts();
    updateContacts(false);
    if ((m_allParticleFlags & ParticleType.b2_viscousParticle) != 0) {
        solveViscous(step);
    }
    if ((m_allParticleFlags & ParticleType.b2_powderParticle) != 0) {
        solvePowder(step);
    }
    if ((m_allParticleFlags & ParticleType.b2_tensileParticle) != 0) {
        solveTensile(step);
    }
    if ((m_allParticleFlags & ParticleType.b2_elasticParticle) != 0) {
        solveElastic(step);
    }
    if ((m_allParticleFlags & ParticleType.b2_springParticle) != 0) {
        solveSpring(step);
    }
    if ((m_allGroupFlags & ParticleGroupType.b2_solidParticleGroup) != 0) {
        solveSolid(step);
    }
    if ((m_allParticleFlags & ParticleType.b2_colorMixingParticle) != 0) {
        solveColorMixing(step);
    }
    solvePressure(step);
    solveDamping(step);
}