OgreNewt_Debugger.cpp

Go to the documentation of this file.

#include "External/ogrenewt/OgreNewt_Stdafx.h"
#include "External/ogrenewt/OgreNewt_Debugger.h"
#include "External/ogrenewt/OgreNewt_World.h"
#include "External/ogrenewt/OgreNewt_Body.h"
#include "External/ogrenewt/OgreNewt_Joint.h"
#include "External/ogrenewt/OgreNewt_Collision.h"
#include <CustomJoint.h>
 
#include <sstream>
 
#include <OgreSceneNode.h>
#include <OgreSceneManager.h>
#include <OgreManualObject.h>
 
namespace OgreNewt
{
 
  // DEUBBER FUNCTIONS
  Debugger::Debugger(const OgreNewt::World* world)
  {
    m_world = world;
    m_debugnode = NULL;
    m_raycastsnode = NULL;
    m_defaultcolor = Ogre::ColourValue::White;
 
    m_recordraycasts = false;
    m_markhitbodies = false;
    m_raycol = Ogre::ColourValue::Green;
    m_convexcol = Ogre::ColourValue::Blue;
    m_hitbodycol = Ogre::ColourValue::Red;
    m_prefilterdiscardedcol = Ogre::ColourValue::Black;
  }
 
  Debugger::~Debugger()
  {
    deInit();
  }
 
  void Debugger::init(Ogre::SceneManager* smgr)
  {
    if (!m_debugnode)
    {
      m_debugnode = smgr->getRootSceneNode()->createChildSceneNode("__OgreNewt__Debugger__Node__");
      m_debugnode->setListener(this);
    }
 
    if (!m_raycastsnode)
    {
      m_raycastsnode = smgr->getRootSceneNode()->createChildSceneNode("__OgreNewt__Raycasts_Debugger__Node__");
      m_raycastsnode->setListener(this);
    }
 
    m_sceneManager = smgr;
  }
 
  void Debugger::deInit()
  {
    clearBodyDebugDataCache();
    if (m_debugnode)
    {
      m_debugnode->setListener(NULL);
      m_debugnode->removeAndDestroyAllChildren();
      m_debugnode->getParentSceneNode()->removeAndDestroyChild(m_debugnode->getName());
      m_debugnode = NULL;
    }
 
 
    clearRaycastsRecorded();
    if (m_raycastsnode)
    {
      m_raycastsnode->setListener(NULL);
      m_raycastsnode->removeAndDestroyAllChildren();
      m_raycastsnode->getParentSceneNode()->removeAndDestroyChild(m_raycastsnode->getName());
      m_raycastsnode = NULL;
    }
  }
 
  void Debugger::nodeDestroyed(const Ogre::Node *node)
  {
    if (node == m_debugnode)
    {
      m_debugnode = NULL;
      clearBodyDebugDataCache();
    }
 
    if (node == m_raycastsnode)
    {
      m_raycastsnode = NULL;
      clearRaycastsRecorded();
    }
  }
 
 
  void Debugger::clearBodyDebugDataCache()
  {
    for (BodyDebugDataMap::iterator it = m_cachemap.begin(); it != m_cachemap.end(); it++)
    {
      Ogre::ManualObject* mo = it->second.m_lines;
      if (mo)
        delete mo;
      OgreNewt::OgreAddons::MovableText *text = it->second.m_text;
      if (text)
        delete text;
    }
    m_cachemap.clear();
  }
 
 
 
  void Debugger::showDebugInformation()
  {
    if (!m_debugnode)
      return;
 
    m_debugnode->removeAllChildren();
 
    // make the new lines.
    for (Body* body = m_world->getFirstBody(); body; body = body->getNext())
    {
      processBody(body);
    }
 
    // display any joint debug information
    NewtonWorldForEachJointDo(m_world->getNewtonWorld(), newtonprocessJoints, this);
 
    // delete old entries
    BodyDebugDataMap newbodymap;
    for (BodyDebugDataMap::iterator it = m_cachemap.begin(); it != m_cachemap.end(); it++)
    {
      if (it->second.m_updated)
        newbodymap.insert(*it);
      else
      {
        Ogre::ManualObject* mo = it->second.m_lines;
        if (mo)
          delete mo;
        OgreNewt::OgreAddons::MovableText *text = it->second.m_text;
        if (text)
          delete text;
      }
    }
    m_cachemap.swap(newbodymap);
  }
 
  void Debugger::hideDebugInformation()
  {
    // erase any existing lines!
    if (m_debugnode)
      m_debugnode->removeAllChildren();
  }
 
  void Debugger::setMaterialColor(const MaterialID* mat, Ogre::ColourValue col)
  {
    m_materialcolors[mat->getID()] = col;
  }
 
  void Debugger::setDefaultColor(Ogre::ColourValue col)
  {
    m_defaultcolor = col;
  }
 
 
  void _CDECL Debugger::newtonprocessJoints(const NewtonJoint* newtonJoint, void* userData)
  {
    Debugger* me = (Debugger*)userData;
    CustomJoint* customJoint = (CustomJoint*)NewtonJointGetUserData(newtonJoint);
    if (customJoint->GetUserData())
      me->processJoint((Joint*)customJoint->GetUserData());
  }
 
 
  void Debugger::processJoint(Joint* joint)
  {
    // show joint info
    joint->showDebugData(m_debugnode);
  }
 
 
  void Debugger::buildDebugObjectFromCollision(Ogre::ManualObject* object, Ogre::ColourValue colour, const NewtonCollision* shape) const
  {
    object->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST);
 
    // set color
    //  if( it != m_materialcolors.end() )
    //          object->colour(it->second);
    //  else
    //          object->colour(m_defaultcolor);
 
    object->colour(colour);
 
    dFloat matrix[16];
    Converters::QuatPosToMatrix(Ogre::Quaternion::IDENTITY, Ogre::Vector3::ZERO, &matrix[0]);
 
    NewtonCollisionForEachPolygonDo(shape, &matrix[0], newtonPerPoly, object);
    object->setCastShadows(false);
    object->end();
  }
 
 
  void Debugger::processBody(OgreNewt::Body* bod)
  {
    NewtonBody* newtonBody = bod->getNewtonBody();
    MaterialIdColorMap::iterator it = m_materialcolors.find(NewtonBodyGetMaterialGroupID(newtonBody));
 
    Ogre::Vector3 pos, vel, omega;
    Ogre::Quaternion ori;
    bod->getVisualPositionOrientation(pos, ori);
 
    vel = bod->getVelocity();
    omega = bod->getOmega();
 
    // ----------- create debug-text ------------
    std::ostringstream oss_name;
    oss_name << "__OgreNewt__Debugger__Body__" << bod << "__";
    std::ostringstream oss_info;
    oss_info.precision(2);
    oss_info.setf(std::ios::fixed, std::ios::floatfield);
    Ogre::Vector3 inertia;
    Ogre::Real mass;
    bod->getMassMatrix(mass, inertia);
 
    oss_info << "[" << bod->getOgreNode()->getName() << "]" << std::endl;
    oss_info << "Mass: " << mass << std::endl;
    oss_info << "Position: " << pos[0] << " x " << pos[1] << " x " << pos[2] << std::endl;
    oss_info << "Velocity: " << vel[0] << " x " << vel[1] << " x " << vel[2] << std::endl;
    oss_info << "Omega: " << omega[0] << " x " << omega[1] << " x " << omega[2] << std::endl;
    oss_info << "Inertia: " << inertia[0] << " x " << inertia[1] << " x " << inertia[2] << std::endl;
 
    // ----------- ------------------ ------------
 
    // look for cached data
    BodyDebugData* data = &m_cachemap[bod];
 
    if (data->m_lastcol == bod->getCollision() && (data->m_scale == bod->getOgreNode()->_getDerivedScale())) // use cached data
    {
      // set new position...
      data->m_node->setPosition(pos);
      data->m_node->setOrientation(ori);
      data->m_updated = 1;
      m_debugnode->addChild(data->m_node);
      data->m_text->setCaption(oss_info.str());
      data->m_text->setLocalTranslation(bod->getAABB().getSize().y * 1.01f * Ogre::Vector3::UNIT_Y);
    }
    else
    {
      data->m_lastcol = bod->getCollision();
      data->m_updated = 1;
      data->m_scale = bod->getOgreNode()->_getDerivedScale();
      if (data->m_node)
      {
        data->m_node->detachAllObjects();
        data->m_node->setPosition(pos);
        data->m_node->setOrientation(ori);
      }
      else
        data->m_node = m_debugnode->createChildSceneNode(pos, ori);
 
      if (data->m_lines)
      {
        data->m_lines->clear();
      }
      else
      {
        std::ostringstream oss;
        oss << "__OgreNewt__Debugger__Lines__" << bod << "__";
        data->m_lines = new Ogre::ManualObject(oss.str());
      }
 
      if (data->m_text)
      {
        data->m_text->setCaption(oss_info.str());
        data->m_text->setLocalTranslation(bod->getAABB().getMaximum().y * 1.01f * Ogre::Vector3::UNIT_Y);
      }
      else
      {
        data->m_text = new OgreNewt::OgreAddons::MovableText(oss_name.str(), oss_info.str(), "BlueHighway-10", bod->getAABB().getSize().y * 0.1f);
        data->m_text->setLocalTranslation(bod->getAABB().getMaximum().y * 1.01f * Ogre::Vector3::UNIT_Y);
        data->m_text->setTextAlignment(OgreNewt::OgreAddons::MovableText::H_LEFT, OgreNewt::OgreAddons::MovableText::V_ABOVE);
      }
 
      data->m_node->attachObject(data->m_text);
 
      buildDebugObjectFromCollision(data->m_lines, m_defaultcolor, NewtonBodyGetCollision(bod->getNewtonBody()));
      data->m_lines->setCastShadows(false);
      data->m_text->setCastShadows(false);
      data->m_node->attachObject(data->m_lines);
    }
  }
 
 
 
  void _CDECL Debugger::newtonPerPoly(void* userData, int vertexCount, const dFloat* faceVertec, int id)
  {
    Ogre::ManualObject* lines = (Ogre::ManualObject*)userData;
    Ogre::Vector3 p0, p1;
 
    if (vertexCount < 2)
      return;
 
    int i = vertexCount - 1;
    p0 = Ogre::Vector3(faceVertec[(i * 3) + 0], faceVertec[(i * 3) + 1], faceVertec[(i * 3) + 2]);
 
 
    for (i = 0; i < vertexCount; i++)
    {
      p1 = Ogre::Vector3(faceVertec[(i * 3) + 0], faceVertec[(i * 3) + 1], faceVertec[(i * 3) + 2]);
 
      lines->position(p0);
      lines->position(p1);
 
      p0 = p1;
    }
  }
 
  // ----------------- raycast-debugging -----------------------
  void Debugger::startRaycastRecording(bool markhitbodies)
  {
    m_recordraycasts = true;
    m_markhitbodies = markhitbodies;
  }
 
  bool Debugger::isRaycastRecording()
  {
    return m_recordraycasts;
  }
 
  bool Debugger::isRaycastRecordingHitBodies()
  {
    return m_markhitbodies;
  }
 
  void Debugger::clearRaycastsRecorded()
  {
    if (m_raycastsnode)
    {
      /*
              while( m_raycastsnode->numAttachedObjects() > 0 )
              {
              delete m_raycastsnode->detachObject((unsigned short)0);
              }
              */
      m_raycastsnode->removeAndDestroyAllChildren();
    }
 
    for (ManualObjectList::iterator it = mRecordedRaycastObjects.begin(); it != mRecordedRaycastObjects.end(); it++)
    {
      delete (*it);
    }
    mRecordedRaycastObjects.clear();
  }
 
  void Debugger::stopRaycastRecording()
  {
    m_recordraycasts = false;
  }
 
  void Debugger::setRaycastRecordingColor(Ogre::ColourValue rayCol, Ogre::ColourValue convexCol, Ogre::ColourValue hitBodyCol, Ogre::ColourValue prefilterDiscardedBodyCol)
  {
    m_raycol = rayCol;
    m_convexcol = convexCol;
    m_hitbodycol = hitBodyCol;
    m_prefilterdiscardedcol = prefilterDiscardedBodyCol;
  }
 
  void Debugger::addRay(const Ogre::Vector3 &startpt, const Ogre::Vector3 &endpt)
  {
    if (!m_raycastsnode)
      return;
 
    static int i = 0;
    std::ostringstream oss;
    oss << "__OgreNewt__Raycast_Debugger__Lines__Raycastline__" << i++ << "__";
    Ogre::ManualObject *line = new Ogre::ManualObject(oss.str());
 
    line->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST);
    line->colour(m_raycol);
    line->position(startpt);
    line->position(endpt);
    line->setCastShadows(false);
    line->end();
 
    mRecordedRaycastObjects.push_back(line);
    m_raycastsnode->attachObject(line);
  }
 
  void Debugger::addConvexRay(const OgreNewt::ConvexCollisionPtr& col, const Ogre::Vector3 &startpt, const Ogre::Quaternion &colori, const Ogre::Vector3 &endpt)
  {
    if (!m_raycastsnode)
      return;
 
    static int i = 0;
    // lines from aab
    std::ostringstream oss;
    oss << "__OgreNewt__Raycast_Debugger__Lines__Convexcastlines__" << i++ << "__";
    Ogre::ManualObject *line = new Ogre::ManualObject(oss.str());
 
    line->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST);
    line->colour(m_convexcol);
 
    /*
      // aab1
      Ogre::AxisAlignedBox aab1 = col->getAABB(colori, startpt);
      const Ogre::Vector3* corners1 = aab1.getAllCorners();
      Ogre::AxisAlignedBox aab2 = col->getAABB(colori, endpt);
      const Ogre::Vector3* corners2 = aab2.getAllCorners();
      for(int i = 0; i < 4; i++)
      {
      line->position(corners1[i]); line->position(corners1[(i+1)%4]);
      line->position(corners1[i+4]); line->position(corners1[(i+1)%4+4]);
      line->position(corners2[i]); line->position(corners2[(i+1)%4]);
      line->position(corners2[i+4]); line->position(corners2[(i+1)%4+4]);
      line->position(corners1[i]); line->position(corners2[i]);
      line->position(corners1[i+4]); line->position(corners2[i+4]);
      }
      line->position(corners1[0]); line->position(corners1[6]);
      line->position(corners1[1]); line->position(corners1[5]);
      line->position(corners1[2]); line->position(corners1[4]);
      line->position(corners1[3]); line->position(corners1[7]);
      line->position(corners2[0]); line->position(corners2[6]);
      line->position(corners2[1]); line->position(corners2[5]);
      line->position(corners2[2]); line->position(corners2[4]);
      line->position(corners2[3]); line->position(corners2[7]);
      */
 
    // bodies
    float matrix[16];
 
    Converters::QuatPosToMatrix(colori, startpt, &matrix[0]);
    NewtonCollisionForEachPolygonDo(col->getNewtonCollision(), &matrix[0], newtonPerPoly, line);
 
    if (endpt != startpt)
    {
      Converters::QuatPosToMatrix(colori, endpt, &matrix[0]);
      NewtonCollisionForEachPolygonDo(col->getNewtonCollision(), &matrix[0], newtonPerPoly, line);
    }
    line->setCastShadows(false);
 
    line->end();
 
    mRecordedRaycastObjects.push_back(line);
    m_raycastsnode->attachObject(line);
  }
 
  void Debugger::addDiscardedBody(const OgreNewt::Body* body)
  {
    if (!m_raycastsnode)
      return;
 
    static int i = 0;
    float matrix[16];
    Ogre::Vector3 pos;
    Ogre::Quaternion ori;
 
    std::ostringstream oss;
    oss << "__OgreNewt__Raycast_Debugger__Lines__DiscardedBody__" << i++ << "__";
    Ogre::ManualObject *line = new Ogre::ManualObject(oss.str());
 
    line->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST);
    line->colour(m_prefilterdiscardedcol);
 
    body->getVisualPositionOrientation(pos, ori);
    Converters::QuatPosToMatrix(ori, pos, &matrix[0]);
    NewtonCollisionForEachPolygonDo(body->getCollision()->getNewtonCollision(), &matrix[0], newtonPerPoly, line);
 
    line->setCastShadows(false);
    line->end();
 
    mRecordedRaycastObjects.push_back(line);
    m_raycastsnode->attachObject(line);
  }
 
  void Debugger::addHitBody(const OgreNewt::Body* body)
  {
    if (!m_raycastsnode)
      return;
 
    static int i = 0;
    float matrix[16];
    Ogre::Vector3 pos;
    Ogre::Quaternion ori;
 
    std::ostringstream oss;
    oss << "__OgreNewt__Raycast_Debugger__Lines__HitBody__" << i++ << "__";
    Ogre::ManualObject *line = new Ogre::ManualObject(oss.str());
 
    line->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST);
    line->colour(m_hitbodycol);
 
    body->getVisualPositionOrientation(pos, ori);
    Converters::QuatPosToMatrix(ori, pos, &matrix[0]);
    NewtonCollisionForEachPolygonDo(body->getCollision()->getNewtonCollision(), &matrix[0], newtonPerPoly, line);
 
    line->setCastShadows(false);
    line->end();
 
    mRecordedRaycastObjects.push_back(line);
    m_raycastsnode->attachObject(line);
  }
 
}   // end namespace OgreNewt