C++ CoordinateFrame类代码示例

CoordinateFrame GLight::frame() const {
    CoordinateFrame f;
    if (rightDirection == Vector3::zero()) {
        // No specified right direction; choose one automatically
        if (position.w == 0) {
            // Directional light
            f.lookAt(-position.xyz());
        } else {
            // Spot light
            f.lookAt(spotDirection);
        }
    } else {
        const Vector3& Z = -spotDirection.direction();
        Vector3 X = rightDirection.direction();

        // Ensure the vectors are not too close together
        while (abs(X.dot(Z)) > 0.9f) {
            X = Vector3::random();
        }

        // Ensure perpendicular
        X -= Z * Z.dot(X);
        const Vector3& Y = Z.cross(X);

        f.rotation.setColumn(Vector3::X_AXIS, X);
        f.rotation.setColumn(Vector3::Y_AXIS, Y);
        f.rotation.setColumn(Vector3::Z_AXIS, Z);
    }
    f.translation = position.xyz();

    return f;
}

std::string toEulerString( const CoordinateFrame &c )
{
    stringstream ss;
    ss<<"pos:  " << toString(c.pos())
      <<", r="<<c.roll()*180.0/M_PI<<"deg"
      <<", p="<<c.pitch()*180.0/M_PI<<"deg"
      <<", y="<<c.yaw()*180.0/M_PI<<"deg";
    return ss.str();
}

std::string toString( const CoordinateFrame &c )
{
    stringstream ss;
    ss << endl;
    ss<<"fwd:  " << toString(c.fwd()) << endl;
    ss<<"left: " << toString(c.left()) << endl;
    ss<<"up:   " << toString(c.up()) << endl;
    ss<<"pos:  " << toString(c.pos());
    return ss.str();
}

//===========================================
bool SoundSystem::set_listener(const CoordinateFrame& frame, const Vector3& vel_vec)
{
  FMOD_VECTOR pos = vec3_to_fmod(frame.translation);
  FMOD_VECTOR vel = vec3_to_fmod(vel_vec);
  FMOD_VECTOR forward = vec3_to_fmod(frame.lookVector());
  FMOD_VECTOR up = vec3_to_fmod(frame.upVector());
  
  //cout << "Listener: " << frame.translation.toString() << endl; 

  FMOD_RESULT result = system->set3DListenerAttributes(0, &pos, &vel, &forward, &up);
  return !error_check(result);
}

void Cylinder::getRandomSurfacePoint(Vector3& p, Vector3& N) const {
    float h = height();
    float r = radius();

    // Create a random point on a standard cylinder and then rotate to the global frame.

    // Relative areas (factor of 2PI already taken out)
    float capRelArea  = square(r) / 2.0f;
    float sideRelArea = r * h;

    float r1 = uniformRandom(0, capRelArea * 2 + sideRelArea);

    if (r1 < capRelArea * 2) {

        // Select a point uniformly at random on a disk
        // @cite http://mathworld.wolfram.com/DiskPointPicking.html
        float a = uniformRandom(0, (float)twoPi());
        float r2 = sqrt(uniformRandom(0, 1)) * r;
        p.x = cos(a) * r2;
        p.z = sin(a) * r2;

        N.x = 0;
        N.z = 0;
        if (r1 < capRelArea) {
            // Top
            p.y = h / 2.0f;
            N.y = 1;
        } else {
            // Bottom
            p.y = -h / 2.0f;
            N.y = -1;
        }
    } else {
        // Side
        float a = uniformRandom(0, (float)twoPi());
        N.x = cos(a);
        N.y = 0;
        N.z = sin(a);
        p.x = N.x * r;
        p.z = N.y * r;
        p.y = uniformRandom(-h / 2.0f, h / 2.0f);
    }

    // Transform to world space
    CoordinateFrame cframe;
    getReferenceFrame(cframe);
    
    p = cframe.pointToWorldSpace(p);
    N = cframe.normalToWorldSpace(N);
}

void NodeTrackerManipulator::computeNodeCenterAndRotation(osg::Vec3d& nodeCenter, osg::Quat& nodeRotation) const
{
    osg::Matrixd localToWorld, worldToLocal;
    computeNodeLocalToWorld(localToWorld);
    computeNodeWorldToLocal(worldToLocal);

    osg::NodePath nodePath;
    if (_trackNodePath.getNodePath(nodePath) && !nodePath.empty())
        nodeCenter = osg::Vec3d(nodePath.back()->getBound().center())*localToWorld;
    else
        nodeCenter = osg::Vec3d(0.0f,0.0f,0.0f)*localToWorld;


    switch(_trackerMode)
    {
        case(NODE_CENTER_AND_AZIM):
        {
            CoordinateFrame coordinateFrame = getCoordinateFrame(nodeCenter);
            osg::Matrixd localToFrame(localToWorld*osg::Matrixd::inverse(coordinateFrame));

            double azim = atan2(-localToFrame(0,1),localToFrame(0,0));
            osg::Quat nodeRotationRelToFrame, rotationOfFrame;
            nodeRotationRelToFrame.makeRotate(-azim,0.0,0.0,1.0);
            rotationOfFrame = coordinateFrame.getRotate();
            nodeRotation = nodeRotationRelToFrame*rotationOfFrame;
            break;
        }
        case(NODE_CENTER_AND_ROTATION):
        {
            // scale the matrix to get rid of any scales before we extract the rotation.
            double sx = 1.0/sqrt(localToWorld(0,0)*localToWorld(0,0) + localToWorld(1,0)*localToWorld(1,0) + localToWorld(2,0)*localToWorld(2,0));
            double sy = 1.0/sqrt(localToWorld(0,1)*localToWorld(0,1) + localToWorld(1,1)*localToWorld(1,1) + localToWorld(2,1)*localToWorld(2,1));
            double sz = 1.0/sqrt(localToWorld(0,2)*localToWorld(0,2) + localToWorld(1,2)*localToWorld(1,2) + localToWorld(2,2)*localToWorld(2,2));
            localToWorld = localToWorld*osg::Matrixd::scale(sx,sy,sz);

            nodeRotation = localToWorld.getRotate();
            break;
        }
        case(NODE_CENTER):
        default:
        {
            CoordinateFrame coordinateFrame = getCoordinateFrame(nodeCenter);
            nodeRotation = coordinateFrame.getRotate();
            break;
        }
    }

}

UprightFrame::UprightFrame(const CoordinateFrame& cframe) {
    Vector3 look = cframe.lookVector();

    yaw = G3D::pi() + atan2(look.x, look.z);
    pitch = asin(look.y);

    translation = cframe.translation;
}

void testCoordinateFrame() {
    printf("CoordinateFrame ");

    {
        // Easy case
        CoordinateFrame c;
        c.lookAt(Vector3(-1, 0, -1));
        float h = c.getHeading();
        debugAssert(fuzzyEq(h, G3D::pi() / 4));
    }

    // Test getHeading at a variety of angles
    for (int i = -175; i <= 175; i += 5) {
        CoordinateFrame c;
        float h = c.getHeading();
        debugAssert(h == 0);

        c.rotation = Matrix3::fromAxisAngle(Vector3::unitY(), toRadians(i));

        h = c.getHeading();
        debugAssert(fuzzyEq(h, toRadians(i)));
    }

    printf("passed\n");
}

void Capsule::getRandomSurfacePoint(Vector3& p, Vector3& N) const {
    float h = height();
    float r = radius();

    // Create a random point on a standard capsule and then rotate to the global frame.

    // Relative areas
    float capRelArea  = sqrt(r) / 2.0f;
    float sideRelArea = r * h;

    float r1 = uniformRandom(0, capRelArea * 2 + sideRelArea);

    if (r1 < capRelArea * 2) {

        // Select a point uniformly at random on a sphere
        N = Sphere(Vector3::zero(), 1).randomSurfacePoint();
        p = N * r;
        p.y += sign(p.y) * h / 2.0f;
    } else {
        // Side
        float a = uniformRandom(0, (float)twoPi());
        N.x = cos(a);
        N.y = 0;
        N.z = sin(a);
        p.x = N.x * r;
        p.z = N.y * r;
        p.y = uniformRandom(-h / 2.0f, h / 2.0f);
    }

    // Transform to world space
    CoordinateFrame cframe;
    getReferenceFrame(cframe);
    
    p = cframe.pointToWorldSpace(p);
    N = cframe.normalToWorldSpace(N);
}

void Demo::insertSpiralSlide() {
    int i;
    for (i = 0; i < 41; ++i) {
        double angle = pi() * i / 10.0;
        double angle2 = pi() * (i - 0.6) / 10.0;

        // Outer spiral
        CoordinateFrame c;
        Box b(Vector3(-1, -1, -.1f), Vector3(1, 1, .1f));
        c.translation = Vector3(cos(angle) * 2.9f, i / 3.5f + 1.5f, sin(angle) * 2.9f);
        c.lookAt(Vector3(cos(angle2) * 1.5f, i / 3.5 + 2.2f, sin(angle2) * 1.5f));
        scene.insertStatic(new BoxObject(c.toWorldSpace(b), (Color3::yellow() + Color3::white()) / 2));

        // Inner inner spiral
        {
            Box b(Vector3(-.3f, -.3f, -.1f), Vector3(.25f, .25f, .1f));
            c.translation = Vector3(cos(angle) * 1.2f, i / 3.5f + 1, sin(angle) * 1.2f);
            c.lookAt(Vector3(cos(angle2) * 3, i / 3.5f + 2, sin(angle2) * 3));
            scene.insertStatic(new BoxObject(c.toWorldSpace(b), (Color3::yellow() + Color3::white()) / 2));
        }
    }

    scene.insertDynamic(new SimSphere(Sphere(Vector3(1.9f, 13, -1), .75f), Vector3(-2,-.5f,-2), Color3::blue()));
}

/**
 Two slanted green ramps.
 */
void Demo::insertRamps() {
    {
        Box b(Vector3(-1, 0, -5), Vector3(1, .25f, 5.5f));
        CoordinateFrame c;
        c.lookAt(Vector3(0, 1, 2));
        c.translation = Vector3(-2.5f, 2.25f, 5.5f);
        scene.insertStatic(new BoxObject(c.toWorldSpace(b), (Color3::green() + Color3::white()) / 2));
    }

    // Corner ramp  
    {
        Box b(Vector3(-1, 0, -5), Vector3(1, .25f, 5.5f));
        CoordinateFrame c;
        c.lookAt(Vector3(-2, 2, -2));
        c.translation = Vector3(-11.2f, 2.85f, -7.2f);
        scene.insertStatic(new BoxObject(c.toWorldSpace(b), (Color3::green() + Color3::white()) / 2));
    }     
}

void VisibleEntity::onPose(Array<shared_ptr<Surface> >& surfaceArray) {

    // We have to pose in order to compute bounds that are used for selection in the editor
    // and collisions in simulation, so pose anyway if not visible,
    // but then roll back.
    debugAssert(isFinite(m_frame.translation.x));
    debugAssert(! isNaN(m_frame.rotation[0][0]));
    const int oldLen = surfaceArray.size();

    const bool boundsChangedSincePreviousFrame = poseModel(surfaceArray);

    // Compute bounds for objects that moved
    if (m_lastAABoxBounds.isEmpty() || boundsChangedSincePreviousFrame || (m_lastChangeTime > m_lastBoundsTime)) {

        m_lastSphereBounds = Sphere(m_frame.translation, 0);
        
        const CFrame& myFrameInverse = frame().inverse();

        m_lastObjectSpaceAABoxBounds = AABox::empty();
        m_lastBoxBoundArray.fastClear();

        // Look at all surfaces produced
        for (int i = oldLen; i < surfaceArray.size(); ++i) {
            AABox b;
            Sphere s;
            const shared_ptr<Surface>& surf = surfaceArray[i];

            // body to world transformation for the surface
            CoordinateFrame cframe;
            surf->getCoordinateFrame(cframe, false);
            debugAssertM(cframe.translation.x == cframe.translation.x, "NaN translation");


            surf->getObjectSpaceBoundingSphere(s);
            s = cframe.toWorldSpace(s);
            m_lastSphereBounds.radius = max(m_lastSphereBounds.radius,
                                            (s.center - m_lastSphereBounds.center).length() + s.radius);


            // Take the entity's frame out of consideration, so that we get tight AA bounds 
            // in the Entity's frame
            CFrame osFrame = myFrameInverse * cframe;

            surf->getObjectSpaceBoundingBox(b);

            m_lastBoxBoundArray.append(cframe.toWorldSpace(b));
            const Box& temp = osFrame.toWorldSpace(b);
            m_lastObjectSpaceAABoxBounds.merge(temp);
        }

        // Box can't represent an empty box, so we make empty boxes into real boxes with zero volume here
        if (m_lastObjectSpaceAABoxBounds.isEmpty()) {
            m_lastObjectSpaceAABoxBounds = AABox(Point3::zero());
            m_lastAABoxBounds = AABox(frame().translation);
        }

        m_lastBoxBounds = frame().toWorldSpace(m_lastObjectSpaceAABoxBounds);
        m_lastBoxBounds.getBounds(m_lastAABoxBounds);
        m_lastBoundsTime = System::time();
    }

    if (! m_visible) {
        // Discard my surfaces if I'm invisible; they were only needed for bounds
        surfaceArray.resize(oldLen, false);
    }
}

void Demo::onGraphics(RenderDevice* rd) {

    LightingParameters lighting(G3D::toSeconds(2, 00, 00, AM), false);
    rd->setProjectionAndCameraMatrix(app->debugCamera);

    // Cyan background
    rd->setColorClearValue(Color3(0.1f, 0.5f, 1.0f));

    rd->clear(app->sky.isNull(), true, true);
    if (app->sky.notNull()) {
        app->sky->render(rd, lighting);
    }

    // Create a light 
    Vector4 wsLight(1.0f, 2.5f, 2.0f, 1.0f);
//    Vector4 wsLight(0,1,0,0);

    // Setup lighting
    rd->enableLighting();
		rd->setLight(0, GLight::directional(lighting.lightDirection, lighting.lightColor));
		rd->setAmbientLightColor(lighting.ambient);

        CoordinateFrame cframe;
        // Rotate the quad
        cframe.rotation = Matrix3::fromAxisAngle(Vector3::unitY(), System::time() * 0.1);

        rd->pushState();
            GPUProgram::ArgList vertexArgs;

            rd->setObjectToWorldMatrix(cframe);

            // Take the light to object space
            Vector4 osLight = cframe.toObjectSpace(wsLight);

            // Take the viewer to object space
            Vector3 osEye = cframe.pointToObjectSpace(app->debugCamera.getCoordinateFrame().translation);

            vertexArgs.set("MVP", rd->getModelViewProjectionMatrix());
            vertexArgs.set("osLight", osLight);
            vertexArgs.set("osEye", osEye);
            rd->setVertexProgram(parallaxVP, vertexArgs);

            GPUProgram::ArgList pixelArgs;
            pixelArgs.set("texture", texture);
            pixelArgs.set("normalMap", normalMap);
            rd->setPixelProgram(parallaxPP, pixelArgs);

            model.render(rd);
        rd->popState();


    rd->disableLighting();

    Draw::sphere(Sphere(wsLight.xyz(), .1f), rd, Color3::white(), Color4::clear());

    if (app->sky.notNull()) {
        app->sky->renderLensFlare(rd, lighting);
    }

    rd->push2D();
        app->debugFont->draw2D(rd, "The surface is a single quad textured with parallax bump mapping and per-pixel shading.", Vector2(10, 10), 10, Color3::white(), Color3::black());
        app->debugFont->draw2D(rd, "Press TAB to toggle to first person camera controls.", Vector2(10, 30), 10, Color3::white(), Color3::black());
    rd->pop2D();
}

void drawFeatureEdges(RenderDevice* renderDevice, const PosedModelRef& model, float creaseAngle) {

    float dotThreshold = max(cosf(creaseAngle), 0.0f);

    bool drawCreases = (creaseAngle <= pi() / 2);

    const Vector3 wsEye = renderDevice->getCameraToWorldMatrix().translation;

    const Array<MeshAlg::Edge>&     edgeArray   = model->weldedEdges();
    const Array<Vector3>&           faceNormal  = model->objectSpaceFaceNormals(drawCreases);
    const Array<MeshAlg::Face>&     faceArray   = model->weldedFaces();
    const Array<Vector3>&           vertexArray = model->objectSpaceGeometry().vertexArray;

    // Work in the object space of the model so we don't
    // have to transform the geometry. 
    const CoordinateFrame           cframe      = model->coordinateFrame();
    const Vector3                   eye         = cframe.pointToObjectSpace(wsEye);

    // Compute backfaces
    static Array<bool> backface;
    backface.resize(faceNormal.size(), DONT_SHRINK_UNDERLYING_ARRAY);
    for (int f = faceNormal.size() - 1; f >= 0; --f) {
        // View vector
        const Vector3 V = (eye - vertexArray[faceArray[f].vertexIndex[0]]);
        backface[f] = faceNormal[f].dot(V) < 0;
    }

    // Find contour edges
    static Array<Vector3> cpuVertexArray;
    cpuVertexArray.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
    for (int e = edgeArray.size() - 1; e >= 0; --e) {
        const MeshAlg::Edge& edge = edgeArray[e];

        const int f0 = edge.faceIndex[0];
        const int f1 = edge.faceIndex[1];

        if (
            // Boundaries:
            (f0 == MeshAlg::Face::NONE) ||
            (f1 == MeshAlg::Face::NONE) ||

            // Contours:
            (backface[f0] ^ backface[f1]) ||

            // Front-face creases:
            (drawCreases &&
             (faceNormal[f0].dot(faceNormal[f1]) <= dotThreshold) && 
             ! (backface[f0] && backface[f1]))) {

            cpuVertexArray.append(
                vertexArray[edge.vertexIndex[0]], 
                vertexArray[edge.vertexIndex[1]]);
        }
    }

    VARAreaRef varArea = VARArea::create(cpuVertexArray.size() * sizeof(Vector3));
    VAR gpuVertexArray(cpuVertexArray, varArea);

    renderDevice->pushState();
        renderDevice->setObjectToWorldMatrix(cframe);
        renderDevice->beginIndexedPrimitives();
            renderDevice->setVertexArray(gpuVertexArray);
            renderDevice->sendSequentialIndices(RenderDevice::LINES, cpuVertexArray.size());
        renderDevice->endIndexedPrimitives();
    renderDevice->popState();
}

void ThirdPersonManipulator::setControlFrame(const CoordinateFrame& c) {
    // Compute the offset for the new control frame, and then
    // change the control frame.
    m_offsetFrame = c.inverse() * frame();
    m_controlFrame = c;
}