Examples with Rot spacegraph.space2d.phys.common.Rot used on opensource projects

Example 11 with Rot

use of spacegraph.space2d.phys.common.Rot in project narchy by automenta.

the class WheelJoint method solvePositionConstraints.

@Override
public boolean solvePositionConstraints(SolverData data) {
    Tuple2f cA = data.positions[m_indexA];
    float aA = data.positions[m_indexA].a;
    Tuple2f cB = data.positions[m_indexB];
    float aB = data.positions[m_indexB].a;
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Tuple2f temp = pool.popVec2();
    qA.set(aA);
    qB.set(aB);
    Rot.mulToOut(qA, temp.set(m_localAnchorA).subbed(m_localCenterA), rA);
    Rot.mulToOut(qB, temp.set(m_localAnchorB).subbed(m_localCenterB), rB);
    d.set(cB).subbed(cA).added(rB).subbed(rA);
    Tuple2f ay = pool.popVec2();
    Rot.mulToOut(qA, m_localYAxisA, ay);
    float sAy = Tuple2f.cross(temp.set(d).added(rA), ay);
    float sBy = Tuple2f.cross(rB, ay);
    float C = Tuple2f.dot(d, ay);
    float k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
    float impulse;
    if (k != 0.0f) {
        impulse = -C / k;
    } else {
        impulse = 0.0f;
    }
    final Tuple2f P = pool.popVec2();
    P.x = impulse * ay.x;
    P.y = impulse * ay.y;
    float LA = impulse * sAy;
    float LB = impulse * sBy;
    cA.x -= m_invMassA * P.x;
    cA.y -= m_invMassA * P.y;
    aA -= m_invIA * LA;
    cB.x += m_invMassB * P.x;
    cB.y += m_invMassB * P.y;
    aB += m_invIB * LB;
    pool.pushVec2(3);
    pool.pushRot(2);
    // 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 Math.abs(C) <= Settings.linearSlop;
}

Example 12 with Rot

use of spacegraph.space2d.phys.common.Rot in project narchy by automenta.

the class CircleShape method computeAABB.

@Override
public final void computeAABB(final AABB aabb, final Transform transform, int childIndex) {
    final Rot tq = transform;
    final Tuple2f tp = transform.pos;
    final float px = tq.c * center.x - tq.s * center.y + tp.x;
    final float py = tq.s * center.x + tq.c * center.y + tp.y;
    aabb.lowerBound.x = px - radius;
    aabb.lowerBound.y = py - radius;
    aabb.upperBound.x = px + radius;
    aabb.upperBound.y = py + radius;
}

Example 13 with Rot

use of spacegraph.space2d.phys.common.Rot in project narchy by automenta.

the class CircleShape method testPoint.

@Override
public final boolean testPoint(final Transform transform, final Tuple2f 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;
    final Tuple2f tp = transform.pos;
    float centerx = -(q.c * center.x - q.s * center.y + tp.x - p.x);
    float centery = -(q.s * center.x + q.c * center.y + tp.y - p.y);
    return centerx * centerx + centery * centery <= radius * radius;
}

Example 14 with Rot

use of spacegraph.space2d.phys.common.Rot in project narchy by automenta.

the class CircleShape method computeDistanceToOut.

@Override
public float computeDistanceToOut(Transform xf, Tuple2f p, int childIndex, v2 normalOut) {
    final Rot xfq = xf;
    float centerx = xfq.c * center.x - xfq.s * center.y + xf.pos.x;
    float centery = xfq.s * center.x + xfq.c * center.y + xf.pos.y;
    float dx = p.x - centerx;
    float dy = p.y - centery;
    float d1 = (float) Math.sqrt(dx * dx + dy * dy);
    normalOut.x = dx * 1 / d1;
    normalOut.y = dy * 1 / d1;
    return d1 - radius;
}

Example 15 with Rot

use of spacegraph.space2d.phys.common.Rot in project narchy by automenta.

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.radius + polyB.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;
    findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
    int count1 = poly1.vertices;
    final Tuple2f[] vertices1 = poly1.vertex;
    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.negated();
    np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1, iv1);
    tangent.negated();
    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);
            Tuple2f out = cp.localPoint;
            final float px = clipPoints2[i].v.x - xf2.pos.x;
            final float py = clipPoints2[i].v.y - xf2.pos.y;
            out.x = (xf2.c * px + xf2.s * py);
            out.y = (-xf2.s * px + xf2.c * py);
            cp.id.set(clipPoints2[i].id);
            if (flip) {
                // Swap features
                cp.id.flip();
            }
            ++pointCount;
        }
    }
    manifold.pointCount = pointCount;
}