Examples with Rot org.jbox2d.common.Rot used on opensource projects

Example 21 with Rot

use of org.jbox2d.common.Rot in project libgdx by libgdx.

the class PositionSolverManifold method initializeVelocityConstraints.

public final void initializeVelocityConstraints() {
    // Warm start.
    for (int i = 0; i < m_count; ++i) {
        ContactVelocityConstraint vc = m_velocityConstraints[i];
        ContactPositionConstraint pc = m_positionConstraints[i];
        float radiusA = pc.radiusA;
        float radiusB = pc.radiusB;
        Manifold manifold = m_contacts[vc.contactIndex].getManifold();
        int indexA = vc.indexA;
        int indexB = vc.indexB;
        float mA = vc.invMassA;
        float mB = vc.invMassB;
        float iA = vc.invIA;
        float iB = vc.invIB;
        Vec2 localCenterA = pc.localCenterA;
        Vec2 localCenterB = pc.localCenterB;
        Vec2 cA = m_positions[indexA].c;
        float aA = m_positions[indexA].a;
        Vec2 vA = m_velocities[indexA].v;
        float wA = m_velocities[indexA].w;
        Vec2 cB = m_positions[indexB].c;
        float aB = m_positions[indexB].a;
        Vec2 vB = m_velocities[indexB].v;
        float wB = m_velocities[indexB].w;
        assert (manifold.pointCount > 0);
        final Rot xfAq = xfA.q;
        final Rot xfBq = xfB.q;
        xfAq.set(aA);
        xfBq.set(aB);
        xfA.p.x = cA.x - (xfAq.c * localCenterA.x - xfAq.s * localCenterA.y);
        xfA.p.y = cA.y - (xfAq.s * localCenterA.x + xfAq.c * localCenterA.y);
        xfB.p.x = cB.x - (xfBq.c * localCenterB.x - xfBq.s * localCenterB.y);
        xfB.p.y = cB.y - (xfBq.s * localCenterB.x + xfBq.c * localCenterB.y);
        worldManifold.initialize(manifold, xfA, radiusA, xfB, radiusB);
        final Vec2 vcnormal = vc.normal;
        vcnormal.x = worldManifold.normal.x;
        vcnormal.y = worldManifold.normal.y;
        int pointCount = vc.pointCount;
        for (int j = 0; j < pointCount; ++j) {
            VelocityConstraintPoint vcp = vc.points[j];
            Vec2 wmPj = worldManifold.points[j];
            final Vec2 vcprA = vcp.rA;
            final Vec2 vcprB = vcp.rB;
            vcprA.x = wmPj.x - cA.x;
            vcprA.y = wmPj.y - cA.y;
            vcprB.x = wmPj.x - cB.x;
            vcprB.y = wmPj.y - cB.y;
            float rnA = vcprA.x * vcnormal.y - vcprA.y * vcnormal.x;
            float rnB = vcprB.x * vcnormal.y - vcprB.y * vcnormal.x;
            float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
            vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
            float tangentx = 1.0f * vcnormal.y;
            float tangenty = -1.0f * vcnormal.x;
            float rtA = vcprA.x * tangenty - vcprA.y * tangentx;
            float rtB = vcprB.x * tangenty - vcprB.y * tangentx;
            float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
            vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
            // Setup a velocity bias for restitution.
            vcp.velocityBias = 0.0f;
            float tempx = vB.x + -wB * vcprB.y - vA.x - (-wA * vcprA.y);
            float tempy = vB.y + wB * vcprB.x - vA.y - (wA * vcprA.x);
            float vRel = vcnormal.x * tempx + vcnormal.y * tempy;
            if (vRel < -Settings.velocityThreshold) {
                vcp.velocityBias = -vc.restitution * vRel;
            }
        }
        // If we have two points, then prepare the block solver.
        if (vc.pointCount == 2) {
            VelocityConstraintPoint vcp1 = vc.points[0];
            VelocityConstraintPoint vcp2 = vc.points[1];
            float rn1A = vcp1.rA.x * vcnormal.y - vcp1.rA.y * vcnormal.x;
            float rn1B = vcp1.rB.x * vcnormal.y - vcp1.rB.y * vcnormal.x;
            float rn2A = vcp2.rA.x * vcnormal.y - vcp2.rA.y * vcnormal.x;
            float rn2B = vcp2.rB.x * vcnormal.y - vcp2.rB.y * vcnormal.x;
            float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
            float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
            float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
            if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) {
                // K is safe to invert.
                vc.K.ex.x = k11;
                vc.K.ex.y = k12;
                vc.K.ey.x = k12;
                vc.K.ey.y = k22;
                vc.K.invertToOut(vc.normalMass);
            } else {
                // The constraints are redundant, just use one.
                // TODO_ERIN use deepest?
                vc.pointCount = 1;
            }
        }
    }
}

Example 22 with Rot

use of org.jbox2d.common.Rot in project libgdx by libgdx.

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;
        Vec2 localCenterA = pc.localCenterA;
        Vec2 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 = 0f;
        float iB = 0f;
        if (indexB == toiIndexA || indexB == toiIndexB) {
            mB = pc.invMassB;
            iB = pc.invIB;
        }
        Vec2 cA = m_positions[indexA].c;
        float aA = m_positions[indexA].a;
        Vec2 cB = m_positions[indexB].c;
        float aB = m_positions[indexB].a;
        // Solve normal constraints
        for (int j = 0; j < pointCount; ++j) {
            final Rot xfAq = xfA.q;
            final Rot xfBq = xfB.q;
            xfAq.set(aA);
            xfBq.set(aB);
            xfA.p.x = cA.x - xfAq.c * localCenterAx + xfAq.s * localCenterAy;
            xfA.p.y = cA.y - xfAq.s * localCenterAx - xfAq.c * localCenterAy;
            xfB.p.x = cB.x - xfBq.c * localCenterBx + xfBq.s * localCenterBy;
            xfB.p.y = cB.y - xfBq.s * localCenterBx - xfBq.c * localCenterBy;
            final PositionSolverManifold psm = psolver;
            psm.initialize(pc, xfA, xfB, j);
            Vec2 normal = psm.normal;
            Vec2 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 23 with Rot

use of org.jbox2d.common.Rot in project libgdx by libgdx.

the class PositionSolverManifold method initialize.

public void initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index) {
    assert (pc.pointCount > 0);
    final Rot xfAq = xfA.q;
    final Rot xfBq = xfB.q;
    final Vec2 pcLocalPointsI = pc.localPoints[index];
    switch(pc.type) {
        case CIRCLES:
            {
                // Transform.mulToOutUnsafe(xfA, pc.localPoint, pointA);
                // Transform.mulToOutUnsafe(xfB, pc.localPoints[0], pointB);
                // normal.set(pointB).subLocal(pointA);
                // normal.normalize();
                //
                // point.set(pointA).addLocal(pointB).mulLocal(.5f);
                // temp.set(pointB).subLocal(pointA);
                // separation = Vec2.dot(temp, normal) - pc.radiusA - pc.radiusB;
                final Vec2 plocalPoint = pc.localPoint;
                final Vec2 pLocalPoints0 = pc.localPoints[0];
                final float pointAx = (xfAq.c * plocalPoint.x - xfAq.s * plocalPoint.y) + xfA.p.x;
                final float pointAy = (xfAq.s * plocalPoint.x + xfAq.c * plocalPoint.y) + xfA.p.y;
                final float pointBx = (xfBq.c * pLocalPoints0.x - xfBq.s * pLocalPoints0.y) + xfB.p.x;
                final float pointBy = (xfBq.s * pLocalPoints0.x + xfBq.c * pLocalPoints0.y) + xfB.p.y;
                normal.x = pointBx - pointAx;
                normal.y = pointBy - pointAy;
                normal.normalize();
                point.x = (pointAx + pointBx) * .5f;
                point.y = (pointAy + pointBy) * .5f;
                final float tempx = pointBx - pointAx;
                final float tempy = pointBy - pointAy;
                separation = tempx * normal.x + tempy * normal.y - pc.radiusA - pc.radiusB;
                break;
            }
        case FACE_A:
            {
                // Rot.mulToOutUnsafe(xfAq, pc.localNormal, normal);
                // Transform.mulToOutUnsafe(xfA, pc.localPoint, planePoint);
                //
                // Transform.mulToOutUnsafe(xfB, pc.localPoints[index], clipPoint);
                // temp.set(clipPoint).subLocal(planePoint);
                // separation = Vec2.dot(temp, normal) - pc.radiusA - pc.radiusB;
                // point.set(clipPoint);
                final Vec2 pcLocalNormal = pc.localNormal;
                final Vec2 pcLocalPoint = pc.localPoint;
                normal.x = xfAq.c * pcLocalNormal.x - xfAq.s * pcLocalNormal.y;
                normal.y = xfAq.s * pcLocalNormal.x + xfAq.c * pcLocalNormal.y;
                final float planePointx = (xfAq.c * pcLocalPoint.x - xfAq.s * pcLocalPoint.y) + xfA.p.x;
                final float planePointy = (xfAq.s * pcLocalPoint.x + xfAq.c * pcLocalPoint.y) + xfA.p.y;
                final float clipPointx = (xfBq.c * pcLocalPointsI.x - xfBq.s * pcLocalPointsI.y) + xfB.p.x;
                final float clipPointy = (xfBq.s * pcLocalPointsI.x + xfBq.c * pcLocalPointsI.y) + xfB.p.y;
                final float tempx = clipPointx - planePointx;
                final float tempy = clipPointy - planePointy;
                separation = tempx * normal.x + tempy * normal.y - pc.radiusA - pc.radiusB;
                point.x = clipPointx;
                point.y = clipPointy;
                break;
            }
        case FACE_B:
            {
                // Rot.mulToOutUnsafe(xfBq, pc.localNormal, normal);
                // Transform.mulToOutUnsafe(xfB, pc.localPoint, planePoint);
                //
                // Transform.mulToOutUnsafe(xfA, pcLocalPointsI, clipPoint);
                // temp.set(clipPoint).subLocal(planePoint);
                // separation = Vec2.dot(temp, normal) - pc.radiusA - pc.radiusB;
                // point.set(clipPoint);
                //
                // // Ensure normal points from A to B
                // normal.negateLocal();
                final Vec2 pcLocalNormal = pc.localNormal;
                final Vec2 pcLocalPoint = pc.localPoint;
                normal.x = xfBq.c * pcLocalNormal.x - xfBq.s * pcLocalNormal.y;
                normal.y = xfBq.s * pcLocalNormal.x + xfBq.c * pcLocalNormal.y;
                final float planePointx = (xfBq.c * pcLocalPoint.x - xfBq.s * pcLocalPoint.y) + xfB.p.x;
                final float planePointy = (xfBq.s * pcLocalPoint.x + xfBq.c * pcLocalPoint.y) + xfB.p.y;
                final float clipPointx = (xfAq.c * pcLocalPointsI.x - xfAq.s * pcLocalPointsI.y) + xfA.p.x;
                final float clipPointy = (xfAq.s * pcLocalPointsI.x + xfAq.c * pcLocalPointsI.y) + xfA.p.y;
                final float tempx = clipPointx - planePointx;
                final float tempy = clipPointy - planePointy;
                separation = tempx * normal.x + tempy * normal.y - pc.radiusA - pc.radiusB;
                point.x = clipPointx;
                point.y = clipPointy;
                normal.x *= -1;
                normal.y *= -1;
            }
            break;
    }
}

Example 24 with Rot

use of org.jbox2d.common.Rot in project libgdx by libgdx.

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;
        Vec2 localCenterA = pc.localCenterA;
        final float localCenterAx = localCenterA.x;
        final float localCenterAy = localCenterA.y;
        float mB = pc.invMassB;
        float iB = pc.invIB;
        Vec2 localCenterB = pc.localCenterB;
        final float localCenterBx = localCenterB.x;
        final float localCenterBy = localCenterB.y;
        int pointCount = pc.pointCount;
        Vec2 cA = m_positions[indexA].c;
        float aA = m_positions[indexA].a;
        Vec2 cB = m_positions[indexB].c;
        float aB = m_positions[indexB].a;
        // Solve normal constraints
        for (int j = 0; j < pointCount; ++j) {
            final Rot xfAq = xfA.q;
            final Rot xfBq = xfB.q;
            xfAq.set(aA);
            xfBq.set(aB);
            xfA.p.x = cA.x - xfAq.c * localCenterAx + xfAq.s * localCenterAy;
            xfA.p.y = cA.y - xfAq.s * localCenterAx - xfAq.c * localCenterAy;
            xfB.p.x = cB.x - xfBq.c * localCenterBx + xfBq.s * localCenterBy;
            xfB.p.y = cB.y - xfBq.s * localCenterBx - xfBq.c * localCenterBy;
            final PositionSolverManifold psm = psolver;
            psm.initialize(pc, xfA, xfB, j);
            final Vec2 normal = psm.normal;
            final Vec2 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 25 with Rot

use of org.jbox2d.common.Rot in project libgdx by libgdx.

the class CircleShape method testPoint.

@Override
public final boolean testPoint(final Transform transform, final Vec2 p) {
    // Rot.mulToOutUnsafe(transform.q, m_p, center);
    // center.addLocal(transform.p);
    //
    // final Vec2 d = center.subLocal(p).negateLocal();
    // return Vec2.dot(d, d) <= m_radius * m_radius;
    final Rot q = transform.q;
    final Vec2 tp = transform.p;
    float centerx = -(q.c * m_p.x - q.s * m_p.y + tp.x - p.x);
    float centery = -(q.s * m_p.x + q.c * m_p.y + tp.y - p.y);
    return centerx * centerx + centery * centery <= m_radius * m_radius;
}