Examples with ContactPoint nars.lab.narclear.jbox2d.ContactPoint used on opensource projects

Example 1 with ContactPoint

use of nars.lab.narclear.jbox2d.ContactPoint in project opennars by opennars.

the class ParticleVelocityQueryCallback method step.

public void step(float timeStep, TestbedSettings settings) {
    // float hz = settings.getSetting(TestbedSettings.Hz).value;
    camera.update(timeStep);
    if (settings.singleStep && !settings.pause) {
        settings.pause = true;
    }
    final DebugDraw debugDraw = model.getDebugDraw();
    m_textLine = 20;
    if (title != null) {
        debugDraw.drawString(camera.getTransform().getExtents().x, 15, title, Color3f.WHITE);
        m_textLine += TEXT_LINE_SPACE;
    }
    if (settings.pause) {
        if (settings.singleStep) {
            settings.singleStep = false;
        } else {
            timeStep = 0;
        }
        debugDraw.drawString(5, m_textLine, "****PAUSED****", Color3f.WHITE);
        m_textLine += TEXT_LINE_SPACE;
    }
    int flags = 0;
    flags += settings.getSetting(TestbedSettings.DrawShapes).enabled ? DebugDraw.e_shapeBit : 0;
    flags += settings.getSetting(TestbedSettings.DrawJoints).enabled ? DebugDraw.e_jointBit : 0;
    flags += settings.getSetting(TestbedSettings.DrawAABBs).enabled ? DebugDraw.e_aabbBit : 0;
    flags += settings.getSetting(TestbedSettings.DrawCOMs).enabled ? DebugDraw.e_centerOfMassBit : 0;
    flags += settings.getSetting(TestbedSettings.DrawTree).enabled ? DebugDraw.e_dynamicTreeBit : 0;
    flags += settings.getSetting(TestbedSettings.DrawWireframe).enabled ? DebugDraw.e_wireframeDrawingBit : 0;
    debugDraw.setFlags(flags);
    m_world.setAllowSleep(settings.getSetting(TestbedSettings.AllowSleep).enabled);
    m_world.setWarmStarting(settings.getSetting(TestbedSettings.WarmStarting).enabled);
    m_world.setSubStepping(settings.getSetting(TestbedSettings.SubStepping).enabled);
    m_world.setContinuousPhysics(settings.getSetting(TestbedSettings.ContinuousCollision).enabled);
    pointCount = 0;
    m_world.step(timeStep, settings.getSetting(TestbedSettings.VelocityIterations).value, settings.getSetting(TestbedSettings.PositionIterations).value);
    // m_world.drawDebugData();
    ((DrawPhy2D) debugDraw).draw(m_world);
    if (timeStep > 0f) {
        ++stepCount;
    }
    // debugDraw.drawString(5, m_textLine, "Engine Info", color4);
    // m_textLine += TEXT_LINE_SPACE;
    debugDraw.drawString(5, m_textLine, "Framerate: " + (int) model.getCalculatedFps(), Color3f.WHITE);
    m_textLine += TEXT_LINE_SPACE;
    if (settings.getSetting(TestbedSettings.DrawStats).enabled) {
        int particleCount = m_world.getParticleCount();
        int groupCount = m_world.getParticleGroupCount();
        debugDraw.drawString(5, m_textLine, "bodies/contacts/joints/proxies/particles/groups = " + m_world.getBodyCount() + "/" + m_world.getContactCount() + "/" + m_world.getJointCount() + "/" + m_world.getProxyCount() + "/" + particleCount + "/" + groupCount, Color3f.WHITE);
        m_textLine += TEXT_LINE_SPACE;
        debugDraw.drawString(5, m_textLine, "World mouse position: " + mouseWorld.toString(), Color3f.WHITE);
        m_textLine += TEXT_LINE_SPACE;
        statsList.clear();
        Profile p = getWorld().getProfile();
        p.toDebugStrings(statsList);
        for (String s : statsList) {
            debugDraw.drawString(5, m_textLine, s, Color3f.WHITE);
            m_textLine += TEXT_LINE_SPACE;
        }
        m_textLine += TEXT_SECTION_SPACE;
    }
    if (settings.getSetting(TestbedSettings.DrawHelp).enabled) {
        debugDraw.drawString(5, m_textLine, "Help", color4);
        m_textLine += TEXT_LINE_SPACE;
        List<String> help = model.getImplSpecificHelp();
        for (String string : help) {
            debugDraw.drawString(5, m_textLine, string, Color3f.WHITE);
            m_textLine += TEXT_LINE_SPACE;
        }
        m_textLine += TEXT_SECTION_SPACE;
    }
    if (!textList.isEmpty()) {
        debugDraw.drawString(5, m_textLine, "Test Info", color4);
        m_textLine += TEXT_LINE_SPACE;
        for (String s : textList) {
            debugDraw.drawString(5, m_textLine, s, Color3f.WHITE);
            m_textLine += TEXT_LINE_SPACE;
        }
        textList.clear();
    }
    if (mouseTracing && mouseJoint == null) {
        float delay = 0.1f;
        acceleration.x = 2 / delay * (1 / delay * (mouseWorld.x - mouseTracerPosition.x) - mouseTracerVelocity.x);
        acceleration.y = 2 / delay * (1 / delay * (mouseWorld.y - mouseTracerPosition.y) - mouseTracerVelocity.y);
        mouseTracerVelocity.x += timeStep * acceleration.x;
        mouseTracerVelocity.y += timeStep * acceleration.y;
        mouseTracerPosition.x += timeStep * mouseTracerVelocity.x;
        mouseTracerPosition.y += timeStep * mouseTracerVelocity.y;
        pshape.m_p.set(mouseTracerPosition);
        pshape.m_radius = 2;
        pcallback.init(m_world, pshape, mouseTracerVelocity);
        pshape.computeAABB(paabb, identity, 0);
        m_world.queryAABB(pcallback, paabb);
    }
    if (mouseJoint != null) {
        mouseJoint.getAnchorB(p1);
        Vec2 p2 = mouseJoint.getTarget();
        debugDraw.drawSegment(p1, p2, mouseColor);
    }
    if (bombSpawning) {
        debugDraw.drawSegment(bombSpawnPoint, bombMousePoint, Color3f.WHITE);
    }
    if (settings.getSetting(TestbedSettings.DrawContactPoints).enabled) {
        final float k_impulseScale = 0.1f;
        final float axisScale = 0.3f;
        for (int i = 0; i < pointCount; i++) {
            ContactPoint point = points[i];
            if (point.state == PointState.ADD_STATE) {
                debugDraw.drawPoint(point.position, 10f, color1);
            } else if (point.state == PointState.PERSIST_STATE) {
                debugDraw.drawPoint(point.position, 5f, color2);
            }
            if (settings.getSetting(TestbedSettings.DrawContactNormals).enabled) {
                p1.set(point.position);
                p2.set(point.normal).mulLocal(axisScale).addLocal(p1);
                debugDraw.drawSegment(p1, p2, color3);
            } else if (settings.getSetting(TestbedSettings.DrawContactImpulses).enabled) {
                p1.set(point.position);
                p2.set(point.normal).mulLocal(k_impulseScale).mulLocal(point.normalImpulse).addLocal(p1);
                debugDraw.drawSegment(p1, p2, color5);
            }
            if (settings.getSetting(TestbedSettings.DrawFrictionImpulses).enabled) {
                Vec2.crossToOutUnsafe(point.normal, 1, tangent);
                p1.set(point.position);
                p2.set(tangent).mulLocal(k_impulseScale).mulLocal(point.tangentImpulse).addLocal(p1);
                debugDraw.drawSegment(p1, p2, color5);
            }
        }
    }
}

Example 2 with ContactPoint

use of nars.lab.narclear.jbox2d.ContactPoint in project opennars by opennars.

the class ParticleVelocityQueryCallback method preSolve.

public void preSolve(Contact contact, Manifold oldManifold) {
    Manifold manifold = contact.getManifold();
    if (manifold.pointCount == 0) {
        return;
    }
    Fixture fixtureA = contact.getFixtureA();
    Fixture fixtureB = contact.getFixtureB();
    Collision.getPointStates(state1, state2, oldManifold, manifold);
    contact.getWorldManifold(worldManifold);
    for (int i = 0; i < manifold.pointCount && pointCount < MAX_CONTACT_POINTS; i++) {
        ContactPoint cp = points[pointCount];
        cp.fixtureA = fixtureA;
        cp.fixtureB = fixtureB;
        cp.position.set(worldManifold.points[i]);
        cp.normal.set(worldManifold.normal);
        cp.state = state2[i];
        cp.normalImpulse = manifold.points[i].normalImpulse;
        cp.tangentImpulse = manifold.points[i].tangentImpulse;
        cp.separation = worldManifold.separations[i];
        ++pointCount;
    }
}