Examples with Body2D spacegraph.space2d.phys.dynamics.Body2D used on opensource projects

Example 11 with Body2D

use of spacegraph.space2d.phys.dynamics.Body2D in project narchy by automenta.

the class PrismaticJoint method getJointSpeed.

/**
 * Get the current joint translation, usually in meters.
 */
public float getJointSpeed() {
    Body2D bA = A;
    Body2D bB = B;
    Tuple2f temp = pool.popVec2();
    Tuple2f rA = pool.popVec2();
    Tuple2f rB = pool.popVec2();
    Tuple2f p1 = pool.popVec2();
    Tuple2f p2 = pool.popVec2();
    Tuple2f d = pool.popVec2();
    Tuple2f axis = pool.popVec2();
    Tuple2f temp2 = pool.popVec2();
    Tuple2f temp3 = pool.popVec2();
    temp.set(m_localAnchorA).subbed(bA.sweep.localCenter);
    Rot.mulToOutUnsafe(bA, temp, rA);
    temp.set(m_localAnchorB).subbed(bB.sweep.localCenter);
    Rot.mulToOutUnsafe(bB, temp, rB);
    p1.set(bA.sweep.c).added(rA);
    p2.set(bB.sweep.c).added(rB);
    d.set(p2).subbed(p1);
    Rot.mulToOutUnsafe(bA, m_localXAxisA, axis);
    Tuple2f vA = bA.vel;
    Tuple2f vB = bB.vel;
    float wA = bA.velAngular;
    float wB = bB.velAngular;
    Tuple2f.crossToOutUnsafe(wA, axis, temp);
    Tuple2f.crossToOutUnsafe(wB, rB, temp2);
    Tuple2f.crossToOutUnsafe(wA, rA, temp3);
    temp2.added(vB).subbed(vA).subbed(temp3);
    float speed = Tuple2f.dot(d, temp) + Tuple2f.dot(axis, temp2);
    pool.pushVec2(9);
    return speed;
}

Example 12 with Body2D

use of spacegraph.space2d.phys.dynamics.Body2D in project narchy by automenta.

the class RevoluteJoint method getJointSpeed.

public float getJointSpeed() {
    final Body2D b1 = A;
    final Body2D b2 = B;
    return b2.velAngular - b1.velAngular;
}

Example 13 with Body2D

use of spacegraph.space2d.phys.dynamics.Body2D in project narchy by automenta.

the class WheelJoint method getJointTranslation.

public float getJointTranslation() {
    Body2D b1 = A;
    Body2D b2 = B;
    Tuple2f p1 = pool.popVec2();
    Tuple2f p2 = pool.popVec2();
    Tuple2f axis = pool.popVec2();
    b1.getWorldPointToOut(m_localAnchorA, p1);
    b2.getWorldPointToOut(m_localAnchorA, p2);
    p2.subbed(p1);
    b1.getWorldVectorToOut(m_localXAxisA, axis);
    float translation = Tuple2f.dot(p2, axis);
    pool.pushVec2(3);
    return translation;
}

Example 14 with Body2D

use of spacegraph.space2d.phys.dynamics.Body2D in project narchy by automenta.

the class PositionSolverManifold method init.

public final void init(ContactSolverDef def) {
    // System.out.println("Initializing contact solver");
    m_step = def.step;
    m_count = def.count;
    if (m_positionConstraints.length < m_count) {
        ContactPositionConstraint[] old = m_positionConstraints;
        m_positionConstraints = new ContactPositionConstraint[MathUtils.max(old.length * 2, m_count)];
        System.arraycopy(old, 0, m_positionConstraints, 0, old.length);
        for (int i = old.length; i < m_positionConstraints.length; i++) {
            m_positionConstraints[i] = new ContactPositionConstraint();
        }
    }
    if (m_velocityConstraints.length < m_count) {
        ContactVelocityConstraint[] old = m_velocityConstraints;
        m_velocityConstraints = new ContactVelocityConstraint[MathUtils.max(old.length * 2, m_count)];
        System.arraycopy(old, 0, m_velocityConstraints, 0, old.length);
        for (int i = old.length; i < m_velocityConstraints.length; i++) {
            m_velocityConstraints[i] = new ContactVelocityConstraint();
        }
    }
    m_positions = def.positions;
    m_velocities = def.velocities;
    m_contacts = def.contacts;
    for (int i = 0; i < m_count; ++i) {
        // System.out.println("contacts: " + m_count);
        final Contact contact = m_contacts[i];
        final Fixture fixtureA = contact.aFixture;
        final Fixture fixtureB = contact.bFixture;
        final Shape shapeA = fixtureA.shape();
        final Shape shapeB = fixtureB.shape();
        final float radiusA = shapeA.radius;
        final float radiusB = shapeB.radius;
        final Body2D bodyA = fixtureA.getBody();
        final Body2D bodyB = fixtureB.getBody();
        final Manifold manifold = contact.getManifold();
        int pointCount = manifold.pointCount;
        assert (pointCount > 0);
        ContactVelocityConstraint vc = m_velocityConstraints[i];
        vc.friction = contact.m_friction;
        vc.restitution = contact.m_restitution;
        vc.tangentSpeed = contact.m_tangentSpeed;
        vc.indexA = bodyA.island;
        vc.indexB = bodyB.island;
        vc.invMassA = bodyA.m_invMass;
        vc.invMassB = bodyB.m_invMass;
        vc.invIA = bodyA.m_invI;
        vc.invIB = bodyB.m_invI;
        vc.contactIndex = i;
        vc.pointCount = pointCount;
        vc.K.setZero();
        vc.normalMass.setZero();
        ContactPositionConstraint pc = m_positionConstraints[i];
        pc.indexA = bodyA.island;
        pc.indexB = bodyB.island;
        pc.invMassA = bodyA.m_invMass;
        pc.invMassB = bodyB.m_invMass;
        pc.localCenterA.set(bodyA.sweep.localCenter);
        pc.localCenterB.set(bodyB.sweep.localCenter);
        pc.invIA = bodyA.m_invI;
        pc.invIB = bodyB.m_invI;
        pc.localNormal.set(manifold.localNormal);
        pc.localPoint.set(manifold.localPoint);
        pc.pointCount = pointCount;
        pc.radiusA = radiusA;
        pc.radiusB = radiusB;
        pc.type = manifold.type;
        // System.out.println("contact point count: " + pointCount);
        for (int j = 0; j < pointCount; j++) {
            ManifoldPoint cp = manifold.points[j];
            VelocityConstraintPoint vcp = vc.points[j];
            if (m_step.warmStarting) {
                // assert(cp.normalImpulse == 0);
                // System.out.println("contact normal impulse: " + cp.normalImpulse);
                vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse;
                vcp.tangentImpulse = m_step.dtRatio * cp.tangentImpulse;
            } else {
                vcp.normalImpulse = 0;
                vcp.tangentImpulse = 0;
            }
            vcp.rA.setZero();
            vcp.rB.setZero();
            vcp.normalMass = 0;
            vcp.tangentMass = 0;
            vcp.velocityBias = 0;
            pc.localPoints[j].x = cp.localPoint.x;
            pc.localPoints[j].y = cp.localPoint.y;
        }
    }
}