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

Example 41 with Tuple2f

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

the class Body2D method getLocalVector.

/**
 * Gets a local vector given a world vector.
 *
 * @param a vector in world coordinates.
 * @return the corresponding local vector.
 */
public final Tuple2f getLocalVector(Tuple2f worldVector) {
    Tuple2f out = new v2();
    getLocalVectorToOut(worldVector, out);
    return out;
}

Example 42 with Tuple2f

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

the class Body2D method getLocalPoint.

/**
 * Gets a local point relative to the body's origin given a world point.
 *
 * @param a point in world coordinates.
 * @return the corresponding local point relative to the body's origin.
 */
public final Tuple2f getLocalPoint(Tuple2f worldPoint) {
    Tuple2f out = new v2();
    getLocalPointToOut(worldPoint, out);
    return out;
}

Example 43 with Tuple2f

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

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;
        Tuple2f localCenterA = pc.localCenterA;
        Tuple2f localCenterB = pc.localCenterB;
        Tuple2f cA = m_positions[indexA];
        float aA = m_positions[indexA].a;
        Tuple2f vA = m_velocities[indexA];
        float wA = m_velocities[indexA].w;
        Tuple2f cB = m_positions[indexB];
        float aB = m_positions[indexB].a;
        Tuple2f vB = m_velocities[indexB];
        float wB = m_velocities[indexB].w;
        assert (manifold.pointCount > 0);
        final Rot xfAq = xfA;
        final Rot xfBq = xfB;
        xfAq.set(aA);
        xfBq.set(aB);
        xfA.pos.x = cA.x - (xfAq.c * localCenterA.x - xfAq.s * localCenterA.y);
        xfA.pos.y = cA.y - (xfAq.s * localCenterA.x + xfAq.c * localCenterA.y);
        xfB.pos.x = cB.x - (xfBq.c * localCenterB.x - xfBq.s * localCenterB.y);
        xfB.pos.y = cB.y - (xfBq.s * localCenterB.x + xfBq.c * localCenterB.y);
        worldManifold.initialize(manifold, xfA, radiusA, xfB, radiusB);
        final Tuple2f 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];
            Tuple2f wmPj = worldManifold.points[j];
            final Tuple2f vcprA = vcp.rA;
            final Tuple2f 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 44 with Tuple2f

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

the class PositionSolverManifold method initialize.

public void initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index) {
    assert (pc.pointCount > 0);
    final Rot xfAq = xfA;
    final Rot xfBq = xfB;
    final Tuple2f 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 Tuple2f plocalPoint = pc.localPoint;
                final Tuple2f pLocalPoints0 = pc.localPoints[0];
                final float pointAx = (xfAq.c * plocalPoint.x - xfAq.s * plocalPoint.y) + xfA.pos.x;
                final float pointAy = (xfAq.s * plocalPoint.x + xfAq.c * plocalPoint.y) + xfA.pos.y;
                final float pointBx = (xfBq.c * pLocalPoints0.x - xfBq.s * pLocalPoints0.y) + xfB.pos.x;
                final float pointBy = (xfBq.s * pLocalPoints0.x + xfBq.c * pLocalPoints0.y) + xfB.pos.y;
                normal.x = pointBx - pointAx;
                normal.y = pointBy - pointAy;
                normal.normalize();
                point.x = (pointAx + pointBx) * 0.5f;
                point.y = (pointAy + pointBy) * 0.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 Tuple2f pcLocalNormal = pc.localNormal;
                final Tuple2f 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.pos.x;
                final float planePointy = (xfAq.s * pcLocalPoint.x + xfAq.c * pcLocalPoint.y) + xfA.pos.y;
                final float clipPointx = (xfBq.c * pcLocalPointsI.x - xfBq.s * pcLocalPointsI.y) + xfB.pos.x;
                final float clipPointy = (xfBq.s * pcLocalPointsI.x + xfBq.c * pcLocalPointsI.y) + xfB.pos.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 Tuple2f pcLocalNormal = pc.localNormal;
                final Tuple2f 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.pos.x;
                final float planePointy = (xfBq.s * pcLocalPoint.x + xfBq.c * pcLocalPoint.y) + xfB.pos.y;
                final float clipPointx = (xfAq.c * pcLocalPointsI.x - xfAq.s * pcLocalPointsI.y) + xfA.pos.x;
                final float clipPointy = (xfAq.s * pcLocalPointsI.x + xfAq.c * pcLocalPointsI.y) + xfA.pos.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 45 with Tuple2f

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

the class PositionSolverManifold method warmStart.

public void warmStart() {
    // Warm start.
    for (int i = 0; i < m_count; ++i) {
        final ContactVelocityConstraint vc = m_velocityConstraints[i];
        int indexA = vc.indexA;
        int indexB = vc.indexB;
        float mA = vc.invMassA;
        float iA = vc.invIA;
        float mB = vc.invMassB;
        float iB = vc.invIB;
        int pointCount = vc.pointCount;
        Tuple2f vA = m_velocities[indexA];
        float wA = m_velocities[indexA].w;
        Tuple2f vB = m_velocities[indexB];
        float wB = m_velocities[indexB].w;
        Tuple2f normal = vc.normal;
        float tangentx = 1.0f * normal.y;
        float tangenty = -1.0f * normal.x;
        for (int j = 0; j < pointCount; ++j) {
            VelocityConstraintPoint vcp = vc.points[j];
            float Px = tangentx * vcp.tangentImpulse + normal.x * vcp.normalImpulse;
            float Py = tangenty * vcp.tangentImpulse + normal.y * vcp.normalImpulse;
            wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);
            vA.x -= Px * mA;
            vA.y -= Py * mA;
            wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px);
            vB.x += Px * mB;
            vB.y += Py * mB;
        }
        m_velocities[indexA].w = wA;
        m_velocities[indexB].w = wB;
    }
}