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

Example 81 with Tuple2f

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

the class Transform method mulToOutUnsafe.

public static final void mulToOutUnsafe(final Transform T, final Tuple2f v, float scale, final Tuple2f out) {
    assert (v != out);
    float vy = v.y * scale;
    float vx = v.x * scale;
    Rot tq = T;
    Tuple2f pos = T.pos;
    float tqs = tq.s;
    float tqc = tq.c;
    out.x = (tqc * vx - tqs * vy) + pos.x;
    out.y = (tqs * vx + tqc * vy) + pos.y;
}

Example 82 with Tuple2f

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

the class Transform method mulToOutUnsafe.

public static final void mulToOutUnsafe(final Transform T, final Tuple2f v, float scale, final GL2 gl) {
    float vy = v.y * scale;
    float vx = v.x * scale;
    Rot tq = T;
    Tuple2f pos = T.pos;
    float tqs = tq.s;
    float tqc = tq.c;
    gl.glVertex2f((tqc * vx - tqs * vy) + pos.x, (tqs * vx + tqc * vy) + pos.y);
}

Example 83 with Tuple2f

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

the class Island method solve.

public void solve(Dynamics2D.Profile profile, TimeStep step, Tuple2f gravity, boolean allowSleep) {
    // System.out.println("Solving Island");
    float h = step.dt;
    // Integrate velocities and apply damping. Initialize the body state.
    for (int i = 0; i < m_bodyCount; ++i) {
        final Body2D b = bodies[i];
        final Sweep bm_sweep = b.sweep;
        final Tuple2f c = bm_sweep.c;
        float a = bm_sweep.a;
        final Tuple2f v = b.vel;
        float w = b.velAngular;
        // Store positions for continuous collision.
        bm_sweep.c0.set(bm_sweep.c);
        bm_sweep.a0 = bm_sweep.a;
        if (b.type == BodyType.DYNAMIC) {
            // Integrate velocities.
            // v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
            v.x += h * (b.m_gravityScale * gravity.x + b.m_invMass * b.force.x);
            v.y += h * (b.m_gravityScale * gravity.y + b.m_invMass * b.force.y);
            w += h * b.m_invI * b.torque;
            // Apply damping.
            // ODE: dv/dt + c * v = 0
            // Solution: v(t) = v0 * exp(-c * t)
            // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v *
            // exp(-c * dt)
            // v2 = exp(-c * dt) * v1
            // Pade approximation:
            // v2 = v1 * 1 / (1 + c * dt)
            v.x *= 1.0f / (1.0f + h * b.m_linearDamping);
            v.y *= 1.0f / (1.0f + h * b.m_linearDamping);
            w *= 1.0f / (1.0f + h * b.m_angularDamping);
        }
        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;
    }
    timer.reset();
    // Solver data
    solverData.step = step;
    solverData.positions = positions;
    solverData.velocities = velocities;
    // Initialize velocity constraints.
    solverDef.step = step;
    solverDef.contacts = contacts;
    solverDef.count = m_contactCount;
    solverDef.positions = positions;
    solverDef.velocities = velocities;
    contactSolver.init(solverDef);
    // System.out.println("island init vel");
    contactSolver.initializeVelocityConstraints();
    if (step.warmStarting) {
        // System.out.println("island warm start");
        contactSolver.warmStart();
    }
    for (int i = 0; i < m_jointCount; ++i) {
        joints[i].initVelocityConstraints(solverData);
    }
    profile.solveInit.accum(timer::getMilliseconds);
    // Solve velocity constraints
    timer.reset();
    // System.out.println("island solving velocities");
    for (int i = 0; i < step.velocityIterations; ++i) {
        for (int j = 0; j < m_jointCount; ++j) {
            joints[j].solveVelocityConstraints(solverData);
        }
        contactSolver.solveVelocityConstraints();
    }
    // Store impulses for warm starting
    contactSolver.storeImpulses();
    profile.solveVelocity.accum(timer::getMilliseconds);
    // Integrate positions
    for (int i = 0; i < m_bodyCount; ++i) {
        final Tuple2f c = positions[i];
        float a = positions[i].a;
        final 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.x *= ratio;
            v.y *= ratio;
        }
        float rotation = h * w;
        if (rotation * rotation > Settings.maxRotationSquared) {
            float ratio = Settings.maxRotation / Math.abs(rotation);
            w *= ratio;
        }
        // Integrate
        c.x += h * v.x;
        c.y += h * v.y;
        a += h * w;
        positions[i].a = a;
        velocities[i].w = w;
    }
    // Solve position constraints
    timer.reset();
    boolean positionSolved = false;
    for (int i = 0; i < step.positionIterations; ++i) {
        boolean contactsOkay = contactSolver.solvePositionConstraints();
        boolean jointsOkay = true;
        for (int j = 0; j < m_jointCount; ++j) {
            boolean jointOkay = joints[j].solvePositionConstraints(solverData);
            jointsOkay = jointsOkay && jointOkay;
        }
        if (contactsOkay && jointsOkay) {
            // Exit early if the position errors are small.
            positionSolved = true;
            break;
        }
    }
    // Copy state buffers back to the bodies
    for (int i = 0; i < m_bodyCount; ++i) {
        Body2D body = bodies[i];
        body.sweep.c.x = positions[i].x;
        body.sweep.c.y = positions[i].y;
        body.sweep.a = positions[i].a;
        body.vel.x = velocities[i].x;
        body.vel.y = velocities[i].y;
        body.velAngular = velocities[i].w;
        body.synchronizeTransform();
    }
    profile.solvePosition.accum(timer::getMilliseconds);
    report(contactSolver.m_velocityConstraints);
    if (allowSleep) {
        float minSleepTime = Float.MAX_VALUE;
        final float linTolSqr = Settings.linearSleepTolerance * Settings.linearSleepTolerance;
        final float angTolSqr = Settings.angularSleepTolerance * Settings.angularSleepTolerance;
        for (int i = 0; i < m_bodyCount; ++i) {
            Body2D b = bodies[i];
            if (b.getType() == BodyType.STATIC) {
                continue;
            }
            if ((b.flags & Body2D.e_autoSleepFlag) == 0 || b.velAngular * b.velAngular > angTolSqr || Tuple2f.dot(b.vel, b.vel) > linTolSqr) {
                b.m_sleepTime = 0.0f;
                minSleepTime = 0.0f;
            } else {
                b.m_sleepTime += h;
                minSleepTime = MathUtils.min(minSleepTime, b.m_sleepTime);
            }
        }
        if (minSleepTime >= Settings.timeToSleep && positionSolved) {
            for (int i = 0; i < m_bodyCount; ++i) {
                Body2D b = bodies[i];
                b.setAwake(false);
            }
        }
    }
}

Example 84 with Tuple2f

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

the class ChainShape method getChildEdge.

/**
 * Get a child edge.
 */
public void getChildEdge(EdgeShape edge, int index) {
    assert (0 <= index && index < m_count - 1);
    edge.radius = radius;
    final Tuple2f v0 = m_vertices[index + 0];
    final Tuple2f v1 = m_vertices[index + 1];
    edge.m_vertex1.x = v0.x;
    edge.m_vertex1.y = v0.y;
    edge.m_vertex2.x = v1.x;
    edge.m_vertex2.y = v1.y;
    if (index > 0) {
        Tuple2f v = m_vertices[index - 1];
        edge.m_vertex0.x = v.x;
        edge.m_vertex0.y = v.y;
        edge.m_hasVertex0 = true;
    } else {
        edge.m_vertex0.x = m_prevVertex.x;
        edge.m_vertex0.y = m_prevVertex.y;
        edge.m_hasVertex0 = m_hasPrevVertex;
    }
    if (index < m_count - 2) {
        Tuple2f v = m_vertices[index + 2];
        edge.m_vertex3.x = v.x;
        edge.m_vertex3.y = v.y;
        edge.m_hasVertex3 = true;
    } else {
        edge.m_vertex3.x = m_nextVertex.x;
        edge.m_vertex3.y = m_nextVertex.y;
        edge.m_hasVertex3 = m_hasNextVertex;
    }
}

Example 85 with Tuple2f

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

the class ChainShape method createChain.

/**
 * Create a chain with isolated end vertices.
 *
 * @param vertices an array of vertices, these are copied
 * @param count    the vertex count
 */
public void createChain(final Tuple2f[] vertices, int count) {
    assert (m_vertices == null && m_count == 0);
    assert (count >= 2);
    m_count = count;
    m_vertices = new Tuple2f[m_count];
    for (int i = 1; i < m_count; i++) {
        Tuple2f v1 = vertices[i - 1];
        Tuple2f v2 = vertices[i];
        // If the code crashes here, it means your vertices are too close together.
        if (MathUtils.distanceSquared(v1, v2) < Settings.linearSlop * Settings.linearSlop) {
            throw new RuntimeException("Vertices of chain shape are too close together");
        }
    }
    for (int i = 0; i < m_count; i++) {
        m_vertices[i] = new Vec2(vertices[i]);
    }
    m_hasPrevVertex = false;
    m_hasNextVertex = false;
    m_prevVertex.setZero();
    m_nextVertex.setZero();
}