random_racer/src/ODETerrainLoader.cpp

Go to the documentation of this file.

#include <vector>

#include <vrs/bounds.h>
#include <vrs/sg/scenething.h>
#include <vrs/translation.h>

#include <vrsode/BoxShape.h>
#include <vrsode/CollisionBody.h>
#include <vrsode/TriangleData.h>
#include <vrsode/TrimeshShape.h>

#include "ControlPointContainer.h"
#include "TerrainGenerator.h"
#include "VRSTerrainLoader.h"

#include "ODETerrainLoader.h"
#include "ODETerrainLoader.tcc"

using namespace VRS;
using namespace vrsode;


namespace random_racer
{

ODETerrainLoader::ODETerrainLoader(
    SO<SceneThing> p_thing,
    SO<VRSTerrainLoader> p_vrsLoader,
    double p_boxSize,
    double p_viewSize) :  TerrainLoader(p_thing, p_boxSize, p_viewSize)
{
    m_vrsLoader = p_vrsLoader;

    const unsigned int lod = 9; // level of detail
    const unsigned int multLod = (lod - 1)*2 + 1;
    const unsigned int multLod2 = (multLod - 1)*2 + 1;
    const unsigned int multLod3 = (multLod2 - 1)*2 + 1;


    m_triangleData = new TriangleData(
            multLod3*multLod3, (multLod3-1)*(multLod3-1)*2*3);


    fillIndexArray(multLod3, m_triangleData->uncommittedIndexArray());

    std::vector<VRS::Vector> temp, temp2(multLod*multLod);

    grabVertices(Vector(0.0, 0.0, 0.0), &temp);

    doubleVertices(temp, temp2, lod, multLod);

    temp.resize(multLod2*multLod2);

    doubleVertices(temp2, temp, multLod, multLod2);

    temp2.resize(multLod3*multLod3);

    doubleVertices(temp, temp2, multLod2, multLod3);


    convertVertices(
        temp2.begin(), temp2.end(), m_triangleData->uncommittedVertexArray());

    m_triangleData->setVertexArrayFill(multLod3*multLod3);
    m_triangleData->setIndexArrayFill((multLod3-1)*(multLod3-1)*2*3);

    m_triangleData->commitDataUpdate();


    // std::cout << "[0] is (abs) " << (temp[0] +
    //         m_vrsLoader->m_translation->getTranslate()) << std::endl;

    m_trimeshBody = new CollisionBody(m_thing,new TrimeshShape(m_triangleData));

    Vector translate = m_vrsLoader->m_translation->getTranslate();
    translate[1] = 0.1;

    m_trimeshBody->setPosition(translate);

    m_trimeshBody->surface()->setSlip1(0.01);
    m_trimeshBody->surface()->setSlip2(0.01);
    m_trimeshBody->surface()->setMu(9999);
}

ODETerrainLoader::~ODETerrainLoader()
{
    m_thing->clear();
}

void
ODETerrainLoader::grabVertices(
    const VRS::Vector& p_position, std::vector<VRS::Vector>* p_target)
{
    m_vrsLoader->lock();


    // const double ctrlPtSpacing  = 8.0;
    const double lodSpacing     = 8.0;
    const unsigned int lod      = 64;


    // LL = lower left

    Vector odeLL = p_position;

    // odeLL[0] = round(odeLL[0]/ctrlPtSpacing)*ctrlPtSpacing;
    // odeLL[2] = round(odeLL[2]/ctrlPtSpacing)*ctrlPtSpacing;

    odeLL[1] = 0.0;

    odeLL[0] -= (m_viewSize/2.0);
    odeLL[2] -= (m_viewSize/2.0);

    // std::cout << "odeLL" << odeLL << std::endl;


    Vector vrsLL = m_vrsLoader->m_vertexIter->data[0] +
            m_vrsLoader->m_translation->getTranslate();

    vrsLL[1] = 0.0;


    Vector diff = odeLL - vrsLL;

    unsigned int offsetX, offsetY, numPts;

    // offsetX = floor(diff[0]/lodSpacing);
    // offsetY = floor(diff[2]/lodSpacing);

    offsetX = round(diff[0]/lodSpacing);
    offsetY = round(diff[2]/lodSpacing);

    numPts  = int(m_viewSize/lodSpacing) + 1;

    // std::cout << "numPts: " << numPts << std::endl
    //           << "odeLL: " << odeLL << std::endl
    //           << "vrsLLrel: " << m_vrsLoader->m_vertexIter->data[0] << std::endl
    //           << "translation: " << 
    //                 m_vrsLoader->m_translation->getTranslate() << std::endl
    //           << "vrsLL: " << vrsLL << std::endl
    //           << "diff: " << diff << std::endl
    //           << "offset: " << offsetX << " "
    //                         << offsetY << std::endl << std::endl;

    unsigned int i, j;

    for(i = offsetX; i < (numPts+offsetX); i++)
        for(j = offsetY; j < (numPts+offsetY); j++)
            p_target->push_back(m_vrsLoader->m_vertexIter->data[j*lod + i]);


    m_vrsLoader->unlock();
}

VRS::Vector
ODETerrainLoader::interpolate(
    const VRS::Vector& p_a,
    const VRS::Vector& p_b)
{
    return (p_a + p_b)*0.5;
}

unsigned int
ODETerrainLoader::indexForTarget(
    unsigned int p_x,
    unsigned int p_y,
    unsigned int p_lod,
    unsigned int p_newLod,
    unsigned int p_offsetX,
    unsigned int p_offsetY)
{
    unsigned int base, oldIndex;

    oldIndex = (p_y*p_lod + p_x);

    base = oldIndex*2 + (p_y*(p_newLod-1));

    return (base+(p_offsetY*p_newLod) + p_offsetX);
}

void
ODETerrainLoader::doubleVertices(
    std::vector<VRS::Vector>& p_source,
    std::vector<VRS::Vector>& p_target,
    unsigned int p_lod,
    unsigned int p_newLod)
{
    unsigned int i, j;
    VRS::Vector p00, p01, p10;


    for(i = 0; i < p_lod; i++)
    {
        for(j = 0; j < p_lod; j++)
        {
            p00 = p_source[ (i*p_lod + j) ];
            
            if(i < (p_lod-1) && j < (p_lod-1))
            {
                p01 = p_source[ ((i+1)*p_lod + j) ];
                p10 = p_source[ (i*p_lod + j + 1) ];

                p_target[indexForTarget(j, i, p_lod, p_newLod, 0, 0)] = p00;

                p_target[indexForTarget(j, i, p_lod, p_newLod, 0, 1)] = 
                    interpolate(p00, p01);

                p_target[indexForTarget(j, i, p_lod, p_newLod, 1, 0)] = 
                    interpolate(p00, p10);

                p_target[indexForTarget(j, i, p_lod, p_newLod, 1, 1)] = 
                    interpolate(p01, p10);
            }
            else
            {
                p_target[indexForTarget(j, i, p_lod, p_newLod, 0, 0)] = p00;

                if(i == p_lod-1)
                {
                    p_target[indexForTarget(j, i, p_lod, p_newLod, 1, 0)] = p00;
                }
                else if(j == p_lod-1)
                {
                    p_target[indexForTarget(j, i, p_lod, p_newLod, 0, 1)] = p00;
                }
            }
        }
    }
}

void
ODETerrainLoader::replaceTerrain(
    const VRS::Vector& p_position, const VRS::Vector& p_center)
{
    m_triangleData->initDataUpdateKeepCurrent();

    const unsigned int lod = 9;
    const unsigned int multLod = (lod - 1)*2 + 1;
    const unsigned int multLod2 = (multLod - 1)*2 + 1;
    const unsigned int multLod3 = (multLod2 - 1)*2 + 1;


    std::vector<VRS::Vector> temp, temp2(multLod*multLod);

    grabVertices(p_center, &temp);

    doubleVertices(temp, temp2, lod, multLod);

    temp.resize(multLod2*multLod2);

    doubleVertices(temp2, temp, multLod, multLod2);

    temp2.resize(multLod3*multLod3);

    doubleVertices(temp, temp2, multLod2, multLod3);

    convertVertices(
        temp2.begin(), temp2.end(), m_triangleData->uncommittedVertexArray());


    m_vrsLoader->lock();
    Vector translate = m_vrsLoader->m_translation->getTranslate();
    m_vrsLoader->unlock();

    translate[1] = 0.1;

    m_trimeshBody->setPosition(translate);

    m_triangleData->setVertexArrayFill(multLod3*multLod3);
    m_triangleData->setIndexArrayFill((multLod3-1)*(multLod3-1)*2*3);
    m_triangleData->commitDataUpdate();
}

} // namespace random_racer

---