(C++) Smooth Alignment of Up-Vector (p.E to floor)

[CuteAlien]

Neither Math nor Quaternions are my strength, but the following code works for me and might be of use for others. The problem i solved with it is was the smooth alignment of objects (like cars) which move over an uneven floor. Originally i just aligned the up-vector directly to the normal of my sliding plane, but this did lead to objects which flip very fast at the borders of hills, so i wrote that smoothed version.

Code: Select all

// mat_: the resulting smooth rotated matrix
// oldMat_: the original matrix
// newUp_,: the new up-vector to which we smooth
// interpolate_: timestep between 0 and 1 used for smoothing (1 to set directly to that vector)
void SmoothAlignToUpVector(irr::core::matrix4 &mat_, const irr::core::matrix4 &oldMat_, const irr::core::vector3df &newUp_, float interpolate_)
{
    core::vector3df up(0, 1, 0);
    oldMat_.rotateVect(up);
    float dot = up.dotProduct(newUp_);
    core::vector3df axis( up.crossProduct(newUp_) );
    core::quaternion quatTarget(axis.X, axis.Y, axis.Z, 1+dot);
    quatTarget.normalize();
    core::quaternion quatSource;
    core::quaternion quatRot;
    quatRot.slerp(quatSource, quatTarget, interpolate_);
    core::vector3df newRot;
    quatRot.toEuler(newRot);
    newRot *= core::RADTODEG;
    mat_.setRotationDegrees(newRot);

    mat_ = mat_*oldMat_;
}

I use the function as follows:

Code: Select all

// float scaleRotation; // is set to smooth value, play around to get good results
// const irr::core::vector3df repulsion; // is set to the normal of my coll.sliding plane
core::matrix4 oldMat;
core::matrix4 matRot;
oldMat.setRotationDegrees(sceneNode->getRotation());
SmoothAlignToUpVector(matRot, oldMat, repulsion, scaleRotation);
seneNode->setRotation(matRot.getRotationDegrees());

[igorfk]

alien,

how is the un-smoothed version of this code?

and how are you getting the "const vector3df repulsion;" value?

[Katsankat]

Can't get it to work. The cube gets stuck when Y rotation is approximately 80 or 275
Other than that it is good.

Test code.
Press 0,1,2, or 3 to set speed.
Press Q or D to turn.
Press ESC to quit.

Code: Select all

 #include <irrlicht.h>
using namespace irr;
using namespace core;
using namespace scene;
using namespace video;
using namespace gui;
 
/*==============================================================================
 
==============================================================================*/
class TestCar
{
 u8 EngineRPM;
 ISceneManager* smgr;
 IMetaTriangleSelector* META;
 bool isInTheAir;
 
public:
 IMeshSceneNode* node;
 
TestCar(IrrlichtDevice* device, vector3df pos, f32 angle, IMetaTriangleSelector* _META)
{
   smgr = device->getSceneManager();
   META = _META;
 
   node = smgr->addCubeSceneNode(250.f, 0, -1, pos);
   node->setRotation(vector3df(0,angle,0));
   node->setScale(vector3df(1.f,.2f,1.f));
   node->setMaterialTexture(0, device->getVideoDriver()->getTexture("../../media/wall.jpg"));
 
   SMaterial* mat = &node->getMaterial(0);
              mat->EmissiveColor = SColor(255, 200,200,200);
              mat->Lighting  = true;
              mat->FogEnable = true;
 
   EngineRPM = 0;
}
 
void setHP(u8 newHP)
{
   EngineRPM = newHP;
}
 
void turn_left(const u32 elapsed)
{
   vector3df rot = node->getRotation();
   rot.Y -= .06f * elapsed;
   //while (rot.Y < 0) rot.Y += 360.f;
   node->setRotation(rot);
}
 
void turn_right(const u32 elapsed)
{
   vector3df rot = node->getRotation();
   rot.Y += .06f * elapsed;
   //while (rot.Y >= 360) rot.Y -= 360.f;
   node->setRotation(rot);
}
 
void SmoothAlignToUpVector(irr::core::matrix4 &mat_, const irr::core::matrix4 &oldMat_,
                     const irr::core::vector3df &newUp_)
{
    core::vector3df up(0, 1, 0);
    oldMat_.rotateVect(up);
    float dot = up.dotProduct(newUp_);
    core::vector3df axis( up.crossProduct(newUp_) );
    core::quaternion quatTarget(axis.X, axis.Y, axis.Z, 1+dot);
    quatTarget.normalize();
    core::quaternion quatSource;
    core::quaternion quatRot;
    quatRot.slerp(quatSource, quatTarget, 0.01f);//interpolate_);
    core::vector3df newRot;
    quatRot.toEuler(newRot);
    newRot *= core::RADTODEG;
    mat_.setRotationDegrees(newRot);
 
    mat_ *= oldMat_;
}
 
void update(const u32 elapsed)
{
  f32 delta = this->EngineRPM * elapsed / 1000.f;
 
  vector3df pos = node->getPosition();
 
  if (EngineRPM != 0)
  {
      vector3df dir(0.f,0.f,EngineRPM);
      matrix4 mat = node->getRelativeTransformation();
      mat.rotateVect(dir);
      pos += dir * elapsed / 1000.f;
  }
 
 
  // align to ground
  core::line3d<f32> line;
 
  line.start = pos;
  line.start.Y += 50.f;
 
  line.end = line.start;
  line.end.Y -= 50.f;
 
  core::vector3df intersection;
  core::triangle3df tri;
  const ISceneNode *n;
  if (smgr->getSceneCollisionManager()->getCollisionPoint(
         line, META, intersection, tri, n))
  {
    isInTheAir = false;
 
    const core::vector3df repulsion = (tri.getNormal().normalize());
    core::matrix4 oldMat;
    core::matrix4 matRot;
    oldMat.setRotationDegrees(node->getRotation());
    SmoothAlignToUpVector(matRot, oldMat, repulsion);
    node->setRotation(matRot.getRotationDegrees());
 
    if (pos.Y < intersection.Y)
    {
      pos.Y = intersection.Y;
    }
  }
  else
  {
    isInTheAir = true;
    if (pos.Y > intersection.Y )
    {
      pos.Y -= .3f*elapsed;
    }
  }
 
  node->setPosition(pos);
}
 
~TestCar()
{
  // remove node
}
};
 
 
/*==============================================================================
 
==============================================================================*/
class myEventReceiver : public IEventReceiver
{
public:
 myEventReceiver()
 {
   for (u32 i=0; i<KEY_KEY_CODES_COUNT; ++i)
     KeyDown[i] = false;
 }
 
 bool KeyDown[KEY_KEY_CODES_COUNT];
 
 bool OnEvent(const SEvent& event)
 {
  if (event.EventType == irr::EET_KEY_INPUT_EVENT)
  {
    KeyDown[event.KeyInput.Key] = event.KeyInput.PressedDown;
    return false;
  }
  return false;
 }
 
 ~myEventReceiver(){}
};
 
/*==============================================================================
  LIGHT
==============================================================================*/
void addLight(IrrlichtDevice* device)
{
  ISceneManager* smgr  = device->getSceneManager();
  IVideoDriver* driver = device->getVideoDriver();
 
  f32 start=2000.0f;
  f32 end=5000.0f;
  f32 density=0.01f;
  bool pixelFog=false;
  bool rangeFog=true;
  driver->setFog(SColor(0, 250,200,100), irr::video::EFT_FOG_LINEAR , start, end, density, pixelFog, rangeFog);
 
  smgr->setShadowColor(SColor(100, 10,10,10));
 
  video::SColorf color(1.f,.47f,.45f, .1f);
 
  ILightSceneNode* sun;
  sun = smgr->addLightSceneNode(0, vector3df(1000.0f, 10000.0f, -1000.0f), color);
  sun->setLightType(ELT_DIRECTIONAL);
  sun->setRadius(10000.0f);
  sun->setRotation(vector3df(1.f, -45.f, 1.f));
}
/*==============================================================================
  TERRAIN
==============================================================================*/
IMetaTriangleSelector* loadTerrain(IrrlichtDevice* device)
{
  ISceneManager* smgr  = device->getSceneManager();
  IVideoDriver* driver = device->getVideoDriver();
 
  IMetaTriangleSelector* META = smgr->createMetaTriangleSelector();
 
  ITerrainSceneNode* terrain;
  terrain = smgr->addTerrainSceneNode("../../media/terrain-heightmap.bmp");
 
  terrain->setPosition(vector3df(0,-100,0));
  terrain->setScale(core::vector3df(256.f, 12.f, 256.f));
 
  terrain->setMaterialTexture(0, driver->getTexture("../../media/terrain-texture.jpg"));
  terrain->scaleTexture(8.0f, 8.0f);
  terrain->setMaterialType(video::EMT_SOLID);
 
  SMaterial* mat = &terrain->getMaterial(0);
             mat->EmissiveColor = SColor(255, 135,105,55);
             mat->Lighting  = true;
             mat->FogEnable = true;
             mat->Shininess = 60.0f;
             mat->SpecularColor = SColor(0, 223,131,0);
 
  // create triangle selector for the terrain
     ITriangleSelector* selector;
     selector = smgr->createTerrainTriangleSelector(terrain, 0);
         terrain->setTriangleSelector(selector);
     META->addTriangleSelector(selector);
     selector->drop();
 
 return META;
}
/*==============================================================================
 
==============================================================================*/
int main()
{
  dimension2d<s32> Res(800,600);
  bool fullscreen = false;
 
  SIrrlichtCreationParameters param;
   param.DriverType = EDT_OPENGL;
   param.AntiAlias = true;
   param.Bits = 32;
   param.Vsync = false;
   param.Stencilbuffer = true,
   param.Fullscreen = fullscreen;
   param.WindowSize = core::dimension2d<s32>(Res.Width, Res.Height);
   IrrlichtDevice *device = createDeviceEx(param);
 
  if (!device) return 1;
 
  ISceneManager* smgr  = device->getSceneManager();
  device->getCursorControl()->setVisible(false);
  device->setWindowCaption(L"test");
 
  // GUI
  IGUIEnvironment* guienv = device->getGUIEnvironment();
 
  IGUIFont* font = guienv->getFont("../../media/bigfont.png");
 
  IGUIStaticText* txt;
  txt = guienv->addStaticText(L"", core::rect<s32>(60,20,Res.Width-20,Res.Height-20),
                                    false, true, 0, 101, false);
  txt->setOverrideColor(video::SColor(0xff597d3d));
  txt->setOverrideFont(font);
 
  // light
  addLight(device);
 
  // terrain
  IMetaTriangleSelector* META = loadTerrain(device);
 
  // car
  TestCar* car = new TestCar(device, vector3df(10500,700,10600), 33, META);
  myEventReceiver receiver;
  device->setEventReceiver(&receiver);
 
 
  // CAMERA
    ICameraSceneNode* cam;
        cam = smgr->addCameraSceneNodeFPS(0, 100.0f, 1.f);
        cam->setPosition(core::vector3df(10000, 900,10000));
        cam->setFarValue(10000.f);
        cam->setTarget(car->node->getPosition());
 
  // RENDERING LOOP ------------------------------------------------------------
 
  IVideoDriver* driver = device->getVideoDriver();
 
  u32 elapsed = 0, nextCheck = 1000;
 
  while(device->run()) 
  {
    if (!device->isWindowActive()) device->sleep(200);
 
    u32 now = device->getTimer()->getTime();
    static u32 then = now;
    elapsed = now - then;
    then = now;
 
    driver->beginScene(true, true, SColor(0,250,200,100));//0xffdddddd));
 
    if (receiver.KeyDown[KEY_ESCAPE]) break;
    else if (receiver.KeyDown[KEY_KEY_Q]) car->turn_left(elapsed);
    else if (receiver.KeyDown[KEY_KEY_D]) car->turn_right(elapsed);
    else if (receiver.KeyDown[KEY_KEY_1]) car->setHP(100);
    else if (receiver.KeyDown[KEY_KEY_2]) car->setHP(400);
    else if (receiver.KeyDown[KEY_KEY_3]) car->setHP(1000);
    else if (receiver.KeyDown[KEY_KEY_0]) car->setHP(0);
 
    car->update(elapsed);
 
    smgr->drawAll();
 
    if (nextCheck < now)
    {
      stringw str = L"   FPS:"; str += driver->getFPS();
              str += L"\ntris:"; str += driver->getPrimitiveCountDrawn();
              str += L"\n ROT:"; str += car->node->getRotation().Y;
              //str += L"\n elapsed:"; str += elapsed;
              txt->setText(str.c_str());
      nextCheck = now + 100;
    }
 
    guienv->drawAll();
    driver->endScene();
  }
 
  device->closeDevice();
 
  return 0;
}
 

Please what am I doing wrong?