rayの描画

c:\ode-0.11.1\myprg\raycast\raycast.cpp

//https://www10.atwiki.jp/bambooflow/?cmd=word&word=ray&type=&pageid=239
//レイによる距離測定
//レイの描画がわからないのでレイだけを取り出して
//表示してみる

//以下3行コメントアウトしても、実行に影響がなかった
//#ifdef WIN32
//#include <windows.h>
//#endif
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>


#ifdef dDOUBLE
  #define dsDrawBox dsDrawBoxD
  #define dsDrawSphere dsDrawSphereD
  #define dsDrawLine dsDrawLineD
#endif

#define MAX_CONTACTS 4 // 最大の衝突検出可能数

static dWorldID world;
static dSpaceID space;
static dJointGroupID contactgroup;

static dBodyID body_box;
static dGeomID geom_box;
static dGeomID geom_ray[4];

static dBodyID body_ball;
static dGeomID geom_ball;


// start simulation - set viewpoint
static void start()
{
  static float xyz[3] = { 1.43f, -2.32f, 1.12f };
  static float hpr[3] = { 110.00f, 1.00f, 0.00f };

  dsSetViewpoint( xyz, hpr );
}

static void command(int cmd)
{
    switch(cmd)
    {
    case 's':                        // sキーを押すと視点，視線を表示
            {
                 float xyz[3], hpr[3];        // 視点, 視線
             dsGetViewpoint(xyz, hpr);    // 視点，視線を取得
             printf("xyz=%4.2f %4.2f %4.2f   ", xyz[0], xyz[1], xyz[2]);
             printf("hpr=%6.2f %6.2f %5.2f \n", hpr[0], hpr[1], hpr[2]);
        }
            break;
    }
}

// simulation loop
static void simLoop( int pause )
{
    // draw box
    {
        dsSetColor( 1.0f, 1.0f, 0.0f );
        dVector3 size;
                //戻り値をsizeに入れる形になっていないのでわかりずらいが
                //sizeを取得しているのだろう
        dGeomBoxGetLengths( geom_box, size );
        dsDrawBox( dGeomGetPosition( geom_box ), dGeomGetRotation( geom_box ), size );
    }

    // draw ray1
    {
            int i;
            for(i=0; i<4; i++)
            {
                if(i==0) dsSetColor( 1.0f, 0.0f, 0.0f );//赤
                if(i==1) dsSetColor( 0.0f, 1.0f, 0.0f );//緑
                if(i==2) dsSetColor( 0.0f, 0.0f, 1.0f );//青
                if(i==3) dsSetColor( 0.0f, 0.0f, 0.0f );//黒

                //以下はrayを書くための魔法の言葉
                //だいだい　「感じ」で分かるので詳しくしらなくてもいい
        dVector3 Origin, Direction;
        dGeomRayGet(geom_ray[i], Origin, Direction);
        dReal Length = dGeomRayGetLength(geom_ray[i]);

        dVector3 End;
        End[0] = Origin[0] + (Direction[0] * Length);
        End[1] = Origin[1] + (Direction[1] * Length);
        End[2] = Origin[2] + (Direction[2] * Length);
        End[3] = Origin[3] + (Direction[3] * Length);

        dsDrawLine(Origin, End);
            }
    }
}


int main( int argc, char* argv[] )
{
    dInitODE();

    // setup pointers to drawstuff callback functions
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start   = &start;
    fn.step    = &simLoop;
    fn.command = &command;
    fn.stop    = 0;
    fn.path_to_textures = "../drawstuff/textures";

    world = dWorldCreate();

    space = dHashSpaceCreate( 0 );
    contactgroup = dJointGroupCreate( 0 );

    // plane create
    {
        dCreatePlane( space, 0, 0, 1, 0 );
    }

    // box creating
    {
        dReal pos[3] = { 0.0, 0.0, 1.5 };
        dReal size[3] = { 0.5, 0.5, 0.5 };

        // body setting
        body_box = dBodyCreate( world );
        dBodySetPosition( body_box, pos[0], pos[1], pos[2] );
        // geom setting
        geom_box = dCreateBox( space, size[0], size[1], size[2] );
        dGeomSetBody( geom_box, body_box );
        // mass setting
        dMass mass;
        dMassSetBoxTotal( &mass, 10.0, size[0], size[1], size[2] ); // 10kg
        dBodySetMass( body_box, &mass );
        // rotation
        dMatrix3 R;
                //x軸を中心に反時計周りに0°まわす
        dRFromAxisAndAngle( R, 1.0, 0.0, 0.0, 0.0*M_PI/180 );
        dBodySetRotation( body_box, R );

        //// fixed joint setting
        //dJointID fixed;
        //fixed = dJointCreateFixed( world, 0 );
        //dJointAttach( fixed, NULL, body_box );
        //dJointSetFixed( fixed );
    }

    // Ray setting
    {
        dReal pos[3] = { 0.5, 0.0, 0.0 };
        dReal length = 3.0;

                //基本的にrayはdGeomSetBodyによって何かのbodyにセットされたら
                //そのbodyのposがrayの始点となるようだ
                //ray0:赤   ray1:緑  ray2:青
                //赤のray { 0.0, 0.0, 1.5 }から上に3.0の長さ　
                //緑のray     dGeomSetOffsetPosition によって
                //         { 0.5, 0.0, 1.5 }から上に3.0の長さ
                //青のray  dRFromAxisAndAngle によって
                //　　　　　{ 0.0, 0.0, 1.5 }からx+方向にに3.0の長さ　

                //ray共通設定
                int i;
                for(i=0; i<3; i++)
                {
                    geom_ray[i] = dCreateRay( space, length );
            dGeomSetBody( geom_ray[i], body_box );  // <= body is box
                }

                //緑のrayの設定
        // position and rotation setting
                // 最初の重心よりrayのgeom（衝突物体）をposの位置だけ
                // 相対的にずらす(始点の位置)
        dGeomSetOffsetPosition( geom_ray[1], pos[0], pos[1], pos[2] );

                //青のrayの設定
                //最初の赤のrayをｙ軸を中心としてｙ+方向の時計方向に90°回転
                //つまりrayがｘ軸方向に出る
        dMatrix3 R2;
        dRFromAxisAndAngle( R2, 0.0, 1.0, 0.0, 90.0*M_PI/180 );
        dGeomSetOffsetRotation( geom_ray[2], R2 );

                //黒のrayの設定
                //これだけはgeomをbody_boxのセットしなかった
                //0.0, 0.0, 0.0　が始点　3.0, 3.0, 3.0が方向だが
                //長さがどうなっているか不明
                geom_ray[3] = dCreateRay( space, length );
        dGeomRaySet( geom_ray[3], 0.0, 0.0, 0.0,  3.0, 3.0, 3.0 );
    }
   


    dsSimulationLoop( argc, argv, 320, 240, &fn );

    dWorldDestroy( world );
    dCloseODE();
    return 0;
}