Examples with Vec2 com.almasb.fxgl.core.math.Vec2 used on opensource projects

Example 36 with Vec2

use of com.almasb.fxgl.core.math.Vec2 in project FXGL by AlmasB.

the class WorldManifold method initialize.

@SuppressWarnings("PMD.UselessParentheses")
public void initialize(final Manifold manifold, final Transform xfA, float radiusA, final Transform xfB, float radiusB) {
    if (manifold.pointCount == 0) {
        return;
    }
    switch(manifold.type) {
        case CIRCLES:
            {
                final Vec2 pointA = pool3;
                final Vec2 pointB = pool4;
                normal.x = 1;
                normal.y = 0;
                Vec2 v = manifold.localPoint;
                // Transform.mulToOutUnsafe(xfA, manifold.localPoint, pointA);
                // Transform.mulToOutUnsafe(xfB, manifold.points[0].localPoint, pointB);
                pointA.x = (xfA.q.c * v.x - xfA.q.s * v.y) + xfA.p.x;
                pointA.y = (xfA.q.s * v.x + xfA.q.c * v.y) + xfA.p.y;
                Vec2 mp0p = manifold.points[0].localPoint;
                pointB.x = (xfB.q.c * mp0p.x - xfB.q.s * mp0p.y) + xfB.p.x;
                pointB.y = (xfB.q.s * mp0p.x + xfB.q.c * mp0p.y) + xfB.p.y;
                if (pointA.distanceSquared(pointB) > JBoxSettings.EPSILON * JBoxSettings.EPSILON) {
                    normal.x = pointB.x - pointA.x;
                    normal.y = pointB.y - pointA.y;
                    normal.getLengthAndNormalize();
                }
                final float cAx = normal.x * radiusA + pointA.x;
                final float cAy = normal.y * radiusA + pointA.y;
                final float cBx = -normal.x * radiusB + pointB.x;
                final float cBy = -normal.y * radiusB + pointB.y;
                points[0].x = (cAx + cBx) * .5f;
                points[0].y = (cAy + cBy) * .5f;
            }
            break;
        case FACE_A:
            {
                final Vec2 planePoint = pool3;
                Rotation.mulToOutUnsafe(xfA.q, manifold.localNormal, normal);
                Transform.mulToOut(xfA, manifold.localPoint, planePoint);
                final Vec2 clipPoint = pool4;
                for (int i = 0; i < manifold.pointCount; i++) {
                    // b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
                    // b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint,
                    // normal)) * normal;
                    // b2Vec2 cB = clipPoint - radiusB * normal;
                    // points[i] = 0.5f * (cA + cB);
                    Transform.mulToOut(xfB, manifold.points[i].localPoint, clipPoint);
                    // use cA as temporary for now
                    // cA.set(clipPoint).subLocal(planePoint);
                    // float scalar = radiusA - Vec2.dot(cA, normal);
                    // cA.set(normal).mulLocal(scalar).addLocal(clipPoint);
                    // cB.set(normal).mulLocal(radiusB).subLocal(clipPoint).negateLocal();
                    // points[i].set(cA).addLocal(cB).mulLocal(0.5f);
                    final float scalar = radiusA - ((clipPoint.x - planePoint.x) * normal.x + (clipPoint.y - planePoint.y) * normal.y);
                    final float cAx = normal.x * scalar + clipPoint.x;
                    final float cAy = normal.y * scalar + clipPoint.y;
                    final float cBx = -normal.x * radiusB + clipPoint.x;
                    final float cBy = -normal.y * radiusB + clipPoint.y;
                    points[i].x = (cAx + cBx) * .5f;
                    points[i].y = (cAy + cBy) * .5f;
                }
            }
            break;
        case FACE_B:
            final Vec2 planePoint = pool3;
            Rotation.mulToOutUnsafe(xfB.q, manifold.localNormal, normal);
            Transform.mulToOut(xfB, manifold.localPoint, planePoint);
            // final Mat22 R = xfB.q;
            // normal.x = R.ex.x * manifold.localNormal.x + R.ey.x * manifold.localNormal.y;
            // normal.y = R.ex.y * manifold.localNormal.x + R.ey.y * manifold.localNormal.y;
            // final Vec2 v = manifold.localPoint;
            // planePoint.x = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y;
            // planePoint.y = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y;
            final Vec2 clipPoint = pool4;
            for (int i = 0; i < manifold.pointCount; i++) {
                // b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
                // b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint,
                // normal)) * normal;
                // b2Vec2 cA = clipPoint - radiusA * normal;
                // points[i] = 0.5f * (cA + cB);
                Transform.mulToOut(xfA, manifold.points[i].localPoint, clipPoint);
                // cB.set(clipPoint).subLocal(planePoint);
                // float scalar = radiusB - Vec2.dot(cB, normal);
                // cB.set(normal).mulLocal(scalar).addLocal(clipPoint);
                // cA.set(normal).mulLocal(radiusA).subLocal(clipPoint).negateLocal();
                // points[i].set(cA).addLocal(cB).mulLocal(0.5f);
                // points[i] = 0.5f * (cA + cB);
                // 
                // clipPoint.x = xfA.p.x + xfA.q.ex.x * manifold.points[i].localPoint.x + xfA.q.ey.x *
                // manifold.points[i].localPoint.y;
                // clipPoint.y = xfA.p.y + xfA.q.ex.y * manifold.points[i].localPoint.x + xfA.q.ey.y *
                // manifold.points[i].localPoint.y;
                final float scalar = radiusB - ((clipPoint.x - planePoint.x) * normal.x + (clipPoint.y - planePoint.y) * normal.y);
                final float cBx = normal.x * scalar + clipPoint.x;
                final float cBy = normal.y * scalar + clipPoint.y;
                final float cAx = -normal.x * radiusA + clipPoint.x;
                final float cAy = -normal.y * radiusA + clipPoint.y;
                points[i].x = (cAx + cBx) * .5f;
                points[i].y = (cAy + cBy) * .5f;
            }
            // Ensure normal points from A to B.
            normal.x = -normal.x;
            normal.y = -normal.y;
            break;
    }
}

Example 37 with Vec2

use of com.almasb.fxgl.core.math.Vec2 in project FXGL by AlmasB.

the class DynamicTree method raycast.

@Override
public void raycast(TreeRayCastCallback callback, RayCastInput input) {
    final Vec2 p1 = input.p1;
    final Vec2 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.getLengthAndNormalize();
    rx = r.x;
    ry = r.y;
    // v is perpendicular to the segment.
    vx = -1f * ry;
    vy = 1f * rx;
    absVx = FXGLMath.abs(vx);
    absVy = FXGLMath.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++] = root;
    while (nodeStackIndex > 0) {
        final DynamicTreeNode node = nodeStack[--nodeStackIndex];
        if (node == null) {
            continue;
        }
        final AABB nodeAABB = node.aabb;
        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 = FXGLMath.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy);
        if (separation > 0.0f) {
            continue;
        }
        if (node.child1 == null) {
            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.id);
            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 {
            if (nodeStack.length - nodeStackIndex - 2 <= 0) {
                DynamicTreeNode[] newBuffer = new DynamicTreeNode[nodeStack.length * 2];
                System.arraycopy(nodeStack, 0, newBuffer, 0, nodeStack.length);
                nodeStack = newBuffer;
            }
            nodeStack[nodeStackIndex++] = node.child1;
            nodeStack[nodeStackIndex++] = node.child2;
        }
    }
}

Example 38 with Vec2

use of com.almasb.fxgl.core.math.Vec2 in project FXGL by AlmasB.

the class RopeJoint method initVelocityConstraints.

@Override
public void initVelocityConstraints(final SolverData data) {
    m_indexA = m_bodyA.m_islandIndex;
    m_indexB = m_bodyB.m_islandIndex;
    m_localCenterA.set(m_bodyA.m_sweep.localCenter);
    m_localCenterB.set(m_bodyB.m_sweep.localCenter);
    m_invMassA = m_bodyA.m_invMass;
    m_invMassB = m_bodyB.m_invMass;
    m_invIA = m_bodyA.m_invI;
    m_invIB = m_bodyB.m_invI;
    Vec2 cA = data.positions[m_indexA].c;
    float aA = data.positions[m_indexA].a;
    Vec2 vA = data.velocities[m_indexA].v;
    float wA = data.velocities[m_indexA].w;
    Vec2 cB = data.positions[m_indexB].c;
    float aB = data.positions[m_indexB].a;
    Vec2 vB = data.velocities[m_indexB].v;
    float wB = data.velocities[m_indexB].w;
    final Rotation qA = pool.popRot();
    final Rotation qB = pool.popRot();
    final Vec2 temp = pool.popVec2();
    qA.set(aA);
    qB.set(aB);
    // Compute the effective masses.
    Rotation.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
    Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
    m_u.set(cB).addLocal(m_rB).subLocal(cA).subLocal(m_rA);
    m_length = m_u.length();
    float C = m_length - m_maxLength;
    if (C > 0.0f) {
        m_state = LimitState.AT_UPPER;
    } else {
        m_state = LimitState.INACTIVE;
    }
    if (m_length > JBoxSettings.linearSlop) {
        m_u.mulLocal(1.0f / m_length);
    } else {
        m_u.setZero();
        m_mass = 0.0f;
        m_impulse = 0.0f;
        return;
    }
    // Compute effective mass.
    float crA = Vec2.cross(m_rA, m_u);
    float crB = Vec2.cross(m_rB, m_u);
    float invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;
    m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
    if (data.step.warmStarting) {
        // Scale the impulse to support a variable time step.
        m_impulse *= data.step.dtRatio;
        float Px = m_impulse * m_u.x;
        float Py = m_impulse * m_u.y;
        vA.x -= m_invMassA * Px;
        vA.y -= m_invMassA * Py;
        wA -= m_invIA * (m_rA.x * Py - m_rA.y * Px);
        vB.x += m_invMassB * Px;
        vB.y += m_invMassB * Py;
        wB += m_invIB * (m_rB.x * Py - m_rB.y * Px);
    } else {
        m_impulse = 0.0f;
    }
    pool.pushRot(2);
    pool.pushVec2(1);
    // data.velocities[m_indexA].v = vA;
    data.velocities[m_indexA].w = wA;
    // data.velocities[m_indexB].v = vB;
    data.velocities[m_indexB].w = wB;
}

Example 39 with Vec2

use of com.almasb.fxgl.core.math.Vec2 in project FXGL by AlmasB.

the class RopeJoint method solvePositionConstraints.

@Override
public boolean solvePositionConstraints(final SolverData data) {
    Vec2 cA = data.positions[m_indexA].c;
    float aA = data.positions[m_indexA].a;
    Vec2 cB = data.positions[m_indexB].c;
    float aB = data.positions[m_indexB].a;
    final Rotation qA = pool.popRot();
    final Rotation qB = pool.popRot();
    final Vec2 u = pool.popVec2();
    final Vec2 rA = pool.popVec2();
    final Vec2 rB = pool.popVec2();
    final Vec2 temp = pool.popVec2();
    qA.set(aA);
    qB.set(aB);
    // Compute the effective masses.
    Rotation.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
    Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
    u.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
    float length = u.getLengthAndNormalize();
    float C = length - m_maxLength;
    C = FXGLMath.clamp(C, 0.0f, JBoxSettings.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);
    pool.pushRot(2);
    pool.pushVec2(4);
    // data.positions[m_indexA].c = cA;
    data.positions[m_indexA].a = aA;
    // data.positions[m_indexB].c = cB;
    data.positions[m_indexB].a = aB;
    return length - m_maxLength < JBoxSettings.linearSlop;
}

Example 40 with Vec2

use of com.almasb.fxgl.core.math.Vec2 in project FXGL by AlmasB.

the class WeldJoint method solveVelocityConstraints.

@Override
public void solveVelocityConstraints(final SolverData data) {
    Vec2 vA = data.velocities[m_indexA].v;
    float wA = data.velocities[m_indexA].w;
    Vec2 vB = data.velocities[m_indexB].v;
    float wB = data.velocities[m_indexB].w;
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    final Vec2 Cdot1 = pool.popVec2();
    final Vec2 P = pool.popVec2();
    final Vec2 temp = pool.popVec2();
    if (m_frequencyHz > 0.0f) {
        float Cdot2 = wB - wA;
        float impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z);
        m_impulse.z += impulse2;
        wA -= iA * impulse2;
        wB += iB * impulse2;
        Vec2.crossToOutUnsafe(wB, m_rB, Cdot1);
        Vec2.crossToOutUnsafe(wA, m_rA, temp);
        Cdot1.addLocal(vB).subLocal(vA).subLocal(temp);
        final Vec2 impulse1 = P;
        Mat33.mul22ToOutUnsafe(m_mass, Cdot1, impulse1);
        impulse1.negateLocal();
        m_impulse.x += impulse1.x;
        m_impulse.y += impulse1.y;
        vA.x -= mA * P.x;
        vA.y -= mA * P.y;
        wA -= iA * Vec2.cross(m_rA, P);
        vB.x += mB * P.x;
        vB.y += mB * P.y;
        wB += iB * Vec2.cross(m_rB, P);
    } else {
        Vec2.crossToOutUnsafe(wA, m_rA, temp);
        Vec2.crossToOutUnsafe(wB, m_rB, Cdot1);
        Cdot1.addLocal(vB).subLocal(vA).subLocal(temp);
        float Cdot2 = wB - wA;
        final Vec3 Cdot = pool.popVec3();
        Cdot.set(Cdot1.x, Cdot1.y, Cdot2);
        final Vec3 impulse = pool.popVec3();
        Mat33.mulToOutUnsafe(m_mass, Cdot, impulse);
        impulse.negateLocal();
        m_impulse.addLocal(impulse);
        P.set(impulse.x, impulse.y);
        vA.x -= mA * P.x;
        vA.y -= mA * P.y;
        wA -= iA * (Vec2.cross(m_rA, P) + impulse.z);
        vB.x += mB * P.x;
        vB.y += mB * P.y;
        wB += iB * (Vec2.cross(m_rB, P) + impulse.z);
        pool.pushVec3(2);
    }
    // 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;
    pool.pushVec2(3);
}