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

Example 1 with Transform

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

the class Collision method collidePolygons.

/**
   * Compute the collision manifold between two polygons.
   * 
   * @param manifold
   * @param polygon1
   * @param xf1
   * @param polygon2
   * @param xf2
   */
public final void collidePolygons(Manifold manifold, final PolygonShape polyA, final Transform xfA, final PolygonShape polyB, final Transform xfB) {
    // Find edge normal of max separation on A - return if separating axis is found
    // Find edge normal of max separation on B - return if separation axis is found
    // Choose reference edge as min(minA, minB)
    // Find incident edge
    // Clip
    // The normal points from 1 to 2
    manifold.pointCount = 0;
    float totalRadius = polyA.m_radius + polyB.m_radius;
    findMaxSeparation(results1, polyA, xfA, polyB, xfB);
    if (results1.separation > totalRadius) {
        return;
    }
    findMaxSeparation(results2, polyB, xfB, polyA, xfA);
    if (results2.separation > totalRadius) {
        return;
    }
    // reference polygon
    final PolygonShape poly1;
    // incident polygon
    final PolygonShape poly2;
    Transform xf1, xf2;
    // reference edge
    int edge1;
    boolean flip;
    final float k_tol = 0.1f * Settings.linearSlop;
    if (results2.separation > results1.separation + k_tol) {
        poly1 = polyB;
        poly2 = polyA;
        xf1 = xfB;
        xf2 = xfA;
        edge1 = results2.edgeIndex;
        manifold.type = ManifoldType.FACE_B;
        flip = true;
    } else {
        poly1 = polyA;
        poly2 = polyB;
        xf1 = xfA;
        xf2 = xfB;
        edge1 = results1.edgeIndex;
        manifold.type = ManifoldType.FACE_A;
        flip = false;
    }
    final Rot xf1q = xf1.q;
    findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
    int count1 = poly1.m_count;
    final Vec2[] vertices1 = poly1.m_vertices;
    final int iv1 = edge1;
    final int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
    v11.set(vertices1[iv1]);
    v12.set(vertices1[iv2]);
    localTangent.x = v12.x - v11.x;
    localTangent.y = v12.y - v11.y;
    localTangent.normalize();
    // Vec2 localNormal = Vec2.cross(dv, 1.0f);
    localNormal.x = 1f * localTangent.y;
    localNormal.y = -1f * localTangent.x;
    // Vec2 planePoint = 0.5f * (v11+ v12);
    planePoint.x = (v11.x + v12.x) * .5f;
    planePoint.y = (v11.y + v12.y) * .5f;
    // Rot.mulToOutUnsafe(xf1.q, localTangent, tangent);
    tangent.x = xf1q.c * localTangent.x - xf1q.s * localTangent.y;
    tangent.y = xf1q.s * localTangent.x + xf1q.c * localTangent.y;
    // Vec2.crossToOutUnsafe(tangent, 1f, normal);
    final float normalx = 1f * tangent.y;
    final float normaly = -1f * tangent.x;
    Transform.mulToOut(xf1, v11, v11);
    Transform.mulToOut(xf1, v12, v12);
    // v11 = Mul(xf1, v11);
    // v12 = Mul(xf1, v12);
    // Face offset
    // float frontOffset = Vec2.dot(normal, v11);
    float frontOffset = normalx * v11.x + normaly * v11.y;
    // Side offsets, extended by polytope skin thickness.
    // float sideOffset1 = -Vec2.dot(tangent, v11) + totalRadius;
    // float sideOffset2 = Vec2.dot(tangent, v12) + totalRadius;
    float sideOffset1 = -(tangent.x * v11.x + tangent.y * v11.y) + totalRadius;
    float sideOffset2 = tangent.x * v12.x + tangent.y * v12.y + totalRadius;
    // Clip incident edge against extruded edge1 side edges.
    // ClipVertex clipPoints1[2];
    // ClipVertex clipPoints2[2];
    int np;
    // Clip to box side 1
    // np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
    tangent.negateLocal();
    np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1, iv1);
    tangent.negateLocal();
    if (np < 2) {
        return;
    }
    // Clip to negative box side 1
    np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
    if (np < 2) {
        return;
    }
    // Now clipPoints2 contains the clipped points.
    manifold.localNormal.set(localNormal);
    manifold.localPoint.set(planePoint);
    int pointCount = 0;
    for (int i = 0; i < Settings.maxManifoldPoints; ++i) {
        // float separation = Vec2.dot(normal, clipPoints2[i].v) - frontOffset;
        float separation = normalx * clipPoints2[i].v.x + normaly * clipPoints2[i].v.y - frontOffset;
        if (separation <= totalRadius) {
            ManifoldPoint cp = manifold.points[pointCount];
            // cp.m_localPoint = MulT(xf2, clipPoints2[i].v);
            Vec2 out = cp.localPoint;
            final float px = clipPoints2[i].v.x - xf2.p.x;
            final float py = clipPoints2[i].v.y - xf2.p.y;
            out.x = (xf2.q.c * px + xf2.q.s * py);
            out.y = (-xf2.q.s * px + xf2.q.c * py);
            cp.id.set(clipPoints2[i].id);
            if (flip) {
                // Swap features
                cp.id.flip();
            }
            ++pointCount;
        }
    }
    manifold.pointCount = pointCount;
}

Example 2 with Transform

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

the class ParticleSystem method createParticleGroup.

public ParticleGroup createParticleGroup(ParticleGroupDef groupDef) {
    float stride = getParticleStride();
    final Transform identity = tempTransform;
    identity.setIdentity();
    Transform transform = tempTransform2;
    transform.setIdentity();
    int firstIndex = m_count;
    if (groupDef.shape != null) {
        final ParticleDef particleDef = tempParticleDef;
        particleDef.flags = groupDef.flags;
        particleDef.color = groupDef.color;
        particleDef.userData = groupDef.userData;
        Shape shape = groupDef.shape;
        transform.set(groupDef.position, groupDef.angle);
        AABB aabb = temp;
        int childCount = shape.getChildCount();
        for (int childIndex = 0; childIndex < childCount; childIndex++) {
            if (childIndex == 0) {
                shape.computeAABB(aabb, identity, childIndex);
            } else {
                AABB childAABB = temp2;
                shape.computeAABB(childAABB, identity, childIndex);
                aabb.combine(childAABB);
            }
        }
        final float upperBoundY = aabb.upperBound.y;
        final float upperBoundX = aabb.upperBound.x;
        for (float y = MathUtils.floor(aabb.lowerBound.y / stride) * stride; y < upperBoundY; y += stride) {
            for (float x = MathUtils.floor(aabb.lowerBound.x / stride) * stride; x < upperBoundX; x += stride) {
                Vec2 p = tempVec;
                p.x = x;
                p.y = y;
                if (shape.testPoint(identity, p)) {
                    Transform.mulToOut(transform, p, p);
                    particleDef.position.x = p.x;
                    particleDef.position.y = p.y;
                    p.subLocal(groupDef.position);
                    Vec2.crossToOutUnsafe(groupDef.angularVelocity, p, particleDef.velocity);
                    particleDef.velocity.addLocal(groupDef.linearVelocity);
                    createParticle(particleDef);
                }
            }
        }
    }
    int lastIndex = m_count;
    ParticleGroup group = new ParticleGroup();
    group.m_system = this;
    group.m_firstIndex = firstIndex;
    group.m_lastIndex = lastIndex;
    group.m_groupFlags = groupDef.groupFlags;
    group.m_strength = groupDef.strength;
    group.m_userData = groupDef.userData;
    group.m_transform.set(transform);
    group.m_destroyAutomatically = groupDef.destroyAutomatically;
    group.m_prev = null;
    group.m_next = m_groupList;
    if (m_groupList != null) {
        m_groupList.m_prev = group;
    }
    m_groupList = group;
    ++m_groupCount;
    for (int i = firstIndex; i < lastIndex; i++) {
        m_groupBuffer[i] = group;
    }
    updateContacts(true);
    if ((groupDef.flags & k_pairFlags) != 0) {
        for (int k = 0; k < m_contactCount; k++) {
            ParticleContact contact = m_contactBuffer[k];
            int a = contact.indexA;
            int b = contact.indexB;
            if (a > b) {
                int temp = a;
                a = b;
                b = temp;
            }
            if (firstIndex <= a && b < lastIndex) {
                if (m_pairCount >= m_pairCapacity) {
                    int oldCapacity = m_pairCapacity;
                    int newCapacity = m_pairCount != 0 ? 2 * m_pairCount : Settings.minParticleBufferCapacity;
                    m_pairBuffer = BufferUtils.reallocateBuffer(Pair.class, m_pairBuffer, oldCapacity, newCapacity);
                    m_pairCapacity = newCapacity;
                }
                Pair pair = m_pairBuffer[m_pairCount];
                pair.indexA = a;
                pair.indexB = b;
                pair.flags = contact.flags;
                pair.strength = groupDef.strength;
                pair.distance = MathUtils.distance(m_positionBuffer.data[a], m_positionBuffer.data[b]);
                m_pairCount++;
            }
        }
    }
    if ((groupDef.flags & k_triadFlags) != 0) {
        VoronoiDiagram diagram = new VoronoiDiagram(lastIndex - firstIndex);
        for (int i = firstIndex; i < lastIndex; i++) {
            diagram.addGenerator(m_positionBuffer.data[i], i);
        }
        diagram.generate(stride / 2);
        createParticleGroupCallback.system = this;
        createParticleGroupCallback.def = groupDef;
        createParticleGroupCallback.firstIndex = firstIndex;
        diagram.getNodes(createParticleGroupCallback);
    }
    if ((groupDef.groupFlags & ParticleGroupType.b2_solidParticleGroup) != 0) {
        computeDepthForGroup(group);
    }
    return group;
}

Example 3 with Transform

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

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();
            Vec2 temp = tempVec;
            Vec2 cross = tempVec2;
            Rot rotation = tempRot;
            rotation.set(step.dt * group.m_angularVelocity);
            Rot.mulToOutUnsafe(rotation, group.m_center, cross);
            temp.set(group.m_linearVelocity).mulLocal(step.dt).addLocal(group.m_center).subLocal(cross);
            tempXf.p.set(temp);
            tempXf.q.set(rotation);
            Transform.mulToOut(tempXf, group.m_transform, group.m_transform);
            final Transform velocityTransform = tempXf2;
            velocityTransform.p.x = step.inv_dt * tempXf.p.x;
            velocityTransform.p.y = step.inv_dt * tempXf.p.y;
            velocityTransform.q.s = step.inv_dt * tempXf.q.s;
            velocityTransform.q.c = step.inv_dt * (tempXf.q.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 4 with Transform

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

the class Body method synchronizeFixtures.

protected final void synchronizeFixtures() {
    final Transform xf1 = pxf;
    // xf1.position = m_sweep.c0 - Mul(xf1.R, m_sweep.localCenter);
    // xf1.q.set(m_sweep.a0);
    // Rot.mulToOutUnsafe(xf1.q, m_sweep.localCenter, xf1.p);
    // xf1.p.mulLocal(-1).addLocal(m_sweep.c0);
    // inlined:
    xf1.q.s = MathUtils.sin(m_sweep.a0);
    xf1.q.c = MathUtils.cos(m_sweep.a0);
    xf1.p.x = m_sweep.c0.x - xf1.q.c * m_sweep.localCenter.x + xf1.q.s * m_sweep.localCenter.y;
    xf1.p.y = m_sweep.c0.y - xf1.q.s * m_sweep.localCenter.x - xf1.q.c * m_sweep.localCenter.y;
    for (Fixture f = m_fixtureList; f != null; f = f.m_next) {
        f.synchronize(m_world.m_contactManager.m_broadPhase, xf1, m_xf);
    }
}

Example 5 with Transform

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

the class WorldRayCastWrapper method drawJoint.

private void drawJoint(Joint joint) {
    Body bodyA = joint.getBodyA();
    Body bodyB = joint.getBodyB();
    Transform xf1 = bodyA.getTransform();
    Transform xf2 = bodyB.getTransform();
    Vec2 x1 = xf1.p;
    Vec2 x2 = xf2.p;
    Vec2 p1 = pool.popVec2();
    Vec2 p2 = pool.popVec2();
    joint.getAnchorA(p1);
    joint.getAnchorB(p2);
    color.set(0.5f, 0.8f, 0.8f);
    switch(joint.getType()) {
        // TODO djm write after writing joints
        case DISTANCE:
            m_debugDraw.drawSegment(p1, p2, color);
            break;
        case PULLEY:
            {
                PulleyJoint pulley = (PulleyJoint) joint;
                Vec2 s1 = pulley.getGroundAnchorA();
                Vec2 s2 = pulley.getGroundAnchorB();
                m_debugDraw.drawSegment(s1, p1, color);
                m_debugDraw.drawSegment(s2, p2, color);
                m_debugDraw.drawSegment(s1, s2, color);
            }
            break;
        case CONSTANT_VOLUME:
        case MOUSE:
            // don't draw this
            break;
        default:
            m_debugDraw.drawSegment(x1, p1, color);
            m_debugDraw.drawSegment(p1, p2, color);
            m_debugDraw.drawSegment(x2, p2, color);
    }
    pool.pushVec2(2);
}