// simulation loop
static void simLoop (int pause)
{
const dReal *pos;
const dReal *R;
// force for the spheres
// find collisions and add contact joints
dSpaceCollide (space,0,&nearCallback);
// step the simulation
dWorldQuickStep (world,0.01);
// remove all contact joints
dJointGroupEmpty (contactgroup);
// redraw sphere at new location
pos = dGeomGetPosition (sphere0_geom);
R = dGeomGetRotation (sphere0_geom);
dsDrawSphere (pos,R,dGeomSphereGetRadius (sphere0_geom));
pos = dGeomGetPosition (sphere1_geom);
R = dGeomGetRotation (sphere1_geom);
dsDrawSphere (pos,R,dGeomSphereGetRadius (sphere1_geom));
pos = dGeomGetPosition (sphere2_geom);
R = dGeomGetRotation (sphere2_geom);
dsDrawSphere (pos,R,dGeomSphereGetRadius (sphere2_geom));
}
/**
* @brief Desenhar o campo da simulacao
*
* Desenha toda a parte grafica da simulacao, como
* as paredes, as linhas, os robos e a bola.
*
*/
void desenharCampo()
{
dVector3 ss;
dsSetColor (0,0,0);
for(int i=0; i < 6; i++) //Paredes
{
dGeomBoxGetLengths (wall[i],ss);
dsDrawBox (dGeomGetPosition(wall[i]), dGeomGetRotation(wall[i]), ss);
}
for(int i=0; i < 3;i++) //Gols
{
dGeomBoxGetLengths (goalL[i],ss);
dsDrawBox (dGeomGetPosition(goalL[i]), dGeomGetRotation(goalL[i]), ss);
dGeomBoxGetLengths (goalR[i],ss);
dsDrawBox (dGeomGetPosition(goalR[i]), dGeomGetRotation(goalR[i]), ss);
}
dsSetColor (1,1,1); //Círculo Central
dsDrawCylinder(dGeomGetPosition(circle),dGeomGetRotation(circle),0.0001,CIRCLE_RADIUS);
for (int i=0; i < 4; i++) //Quinas
{
dGeomBoxGetLengths (triangle[i],ss);
dsDrawBox (dGeomGetPosition(triangle[i]), dGeomGetRotation(triangle[i]), ss);
}
}
void CODEGeom::get_global_form_bt(Fmatrix& form)
{
dMULTIPLY0_331 ((dReal*)(&form.c),dGeomGetRotation(m_geom_transform),dGeomGetPosition(geom()));
form.c.add(*((const Fvector*)dGeomGetPosition(m_geom_transform)));
dMULTIPLY3_333 ((dReal*)(&form),dGeomGetRotation(m_geom_transform),dGeomGetRotation(geom()));
//PHDynamicData::DMXtoFMX((dReal*)(&form),form);
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!draw_geom){
return;
}
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
/*
// cylinder option not yet implemented
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
*/
else if (type == dGeomTransformClass) {
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMULTIPLY0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R,0);
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R)
{
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCCylinderClass) {
dReal radius,length;
dGeomCCylinderGetParams (g,&radius,&length);
dsDrawCappedCylinder (pos,R,length,radius);
}
else if (type == dGeomTransformClass) {
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMULTIPLY0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R);
}
}
/*******************************************************************************
Function to draw a geometry object.
*******************************************************************************/
void GOdeObject::drawGeom( dGeomID g, const dReal *position, const dReal *orientation ) const
{
if( !g ) //If the geometry object is missing, end the function.
return;
if( !position ) //Position was not passed?
position = dGeomGetPosition( g ); //Then, get the geometry position.
if( !orientation ) //Orientation was not given?
orientation = dGeomGetRotation( g ); //And get existing geometry orientation.
int type = dGeomGetClass( g ); //Get the type of geometry.
if( type == dBoxClass ) //Is it a box?
{
dReal sides[3];
dGeomBoxGetLengths( g, sides ); //Get length of sides.
renderBox( sides, position, orientation ); //Render the actual box in environment.
}
if( type == dSphereClass ) //Is it a sphere?
{
dReal radius;
radius = dGeomSphereGetRadius( g ); //Get the radius.
renderSphere( radius, position, orientation ); //Render sphere in environment.
}
if( type == dCapsuleClass )
{
dReal radius;
dReal length;
dGeomCapsuleGetParams( g, &radius, &length ); //Get both radius and length.
renderCapsule( radius, length, position, orientation ); //Render capsule in environment.
}
if( type == dGeomTransformClass ) //Is it an embeded geom in a composite body.
{
dGeomID g2 = dGeomTransformGetGeom( g ); //Get the actual geometry inside the wrapper.
const dReal *position2 = dGeomGetPosition( g2 ); //Get position and orientation of wrapped geometry.
const dReal *orientation2 = dGeomGetRotation( g2 );
dVector3 actualPosition; //Real world coordinated position and orientation
dMatrix3 actualOrientation; //of the wrapped geometry.
dMultiply0_331( actualPosition, orientation, position2 ); //Get world coordinates of geometry position.
actualPosition[0] += position[0];
actualPosition[1] += position[1];
actualPosition[2] += position[2];
dMultiply0_333( actualOrientation, orientation, orientation2 ); //Get world coordinates of geom orientation.
drawGeom( g2, actualPosition, actualOrientation ); //Draw embeded geometry.
}
}
static void computeFinalTx(dGeomID geom_transform,dReal* final_pos,dReal* final_R)
{
R_ASSERT2(dGeomGetClass(geom_transform)==dGeomTransformClass,"is not a geom transform");
dGeomID obj=dGeomTransformGetGeom(geom_transform);
const dReal *R =dGeomGetRotation(geom_transform);
const dReal *pos=dGeomGetPosition(geom_transform);
dMULTIPLY0_331 (final_pos,R,dGeomGetPosition(obj));
final_pos[0] += pos[0];
final_pos[1] += pos[1];
final_pos[2] += pos[2];
dMULTIPLY0_333 (final_R,R,dGeomGetRotation(obj));
}
void simLoop (int pause)
{
static bool DumpInfo=true;
const dReal ss[3] = {0.02,0.02,0.02};
dContactGeom contacts[8];
int contactcount = dCollideConvexConvex(geoms[0],geoms[1],8,contacts,sizeof(dContactGeom));
//fprintf(stdout,"Contact Count %d\n",contactcount);
const dReal* pos;
const dReal* R;
dsSetTexture (DS_WOOD);
pos = dGeomGetPosition (geoms[0]);
R = dGeomGetRotation (geoms[0]);
dsSetColor (0.6f,0.6f,1);
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
pos = dGeomGetPosition (geoms[1]);
R = dGeomGetRotation (geoms[1]);
dsSetColor (0.4f,1,1);
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
/*if (show_contacts) */
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColor (1.0f,0,0);
for(int i=0;i<contactcount;++i)
{
if(DumpInfo)
{
//DumpInfo=false;
fprintf(stdout,"Contact %d Normal %f,%f,%f Depth %f\n",
i,
contacts[i].normal[0],
contacts[i].normal[1],
contacts[i].normal[2],
contacts[i].depth);
}
dsDrawBox (contacts[i].pos,RI,ss);
}
if(DumpInfo)
DumpInfo=false;
}
void DrawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
// If the geom ID is missing then return immediately.
if (!g)
{
return;
}
// If there was no position vector supplied then get the existing position.
if (!pos)
{
pos = dGeomGetPosition (g);
}
// If there was no rotation matrix given then get the existing rotation.
if (!R)
{
R = dGeomGetRotation (g);
}
// Get the geom's class type.
int type = dGeomGetClass (g);
if (type == dBoxClass)
{
// Create a temporary array of floats to hold the box dimensions.
dReal sides[3];
dGeomBoxGetLengths(g, sides);
// Now to actually render the box we make a call to DrawBox, passing the geoms dimensions, position vector and
// rotation matrix. And this function is the subject of our next discussion.
// DrawBox(sides, pos, R);
}
}
/*** ゴールの描画 ***/
static void drawGoal()
{
dsSetTexture(DS_NONE);
for (int i = 0; i < GOAL_PARTS_NUM; i++)
{
if (i < 4) dsSetColor(1.3, 1.3, 1.3);
else dsSetColor(1.3, 1.3, 0.0);
dsDrawBox(dGeomGetPosition(goal_parts[i]),
dGeomGetRotation(goal_parts[i]),GOAL_SIDES[i]);
if (i < 4) dsSetColor(1.3, 1.3, 1.3);
else dsSetColor(0.0, 0.0, 1.3);
dsDrawBox(dGeomGetPosition(goal_parts2[i]),
dGeomGetRotation(goal_parts2[i]),GOAL_SIDES[i]);
}
}
int dcTriListCollider::CollideBox(dxGeom* Box, int Flags, dContactGeom* Contacts, int Stride)
{
Fvector AABB;
dVector3 BoxSides;
dGeomBoxGetLengths(Box,BoxSides);
dReal* R=const_cast<dReal*>(dGeomGetRotation(Box));
AABB.x=(dFabs(BoxSides[0]*R[0])+dFabs(BoxSides[1]*R[1])+dFabs(BoxSides[2]*R[2]))/2.f +10.f*EPS_L;
AABB.y=(dFabs(BoxSides[0]*R[4])+dFabs(BoxSides[1]*R[5])+dFabs(BoxSides[2]*R[6]))/2.f +10.f*EPS_L;
AABB.z=(dFabs(BoxSides[0]*R[8])+dFabs(BoxSides[1]*R[9])+dFabs(BoxSides[2]*R[10]))/2.f+10.f*EPS_L;
dBodyID box_body=dGeomGetBody(Box);
if(box_body) {
const dReal*velocity=dBodyGetLinearVel(box_body);
AABB.x+=dFabs(velocity[0])*0.04f;
AABB.y+=dFabs(velocity[1])*0.04f;
AABB.z+=dFabs(velocity[2])*0.04f;
}
BoxTri bt(*this);
return dSortTriPrimitiveCollide
(bt,
Box,
Geometry,
Flags,
Contacts,
Stride,
AABB
);
}
void drawGeom( dGeomID g, int colored = 0 )
{
if( !g ) //If the geometry object is missing, end the function.
return;
const dReal *position; //Define pointers to internal positions and orientations.
const dReal *orientation; //Pointers to constant objects (so the objects will not change).
int type = dGeomGetClass( g ); //Get the type of geometry.
position = dGeomGetPosition( g ); //Then, get the geometry position.
orientation = dGeomGetRotation( g ); //And get existing geometry orientation.
if( type == dBoxClass ) //Is it a box?
{
dReal sides[3];
dGeomBoxGetLengths( g, sides ); //Get length of sides.
renderBox( sides, position, orientation, colored ); //Render the actual box in environment.
}
else if (type == dCylinderClass)
{
dReal radius, length;
dGeomCylinderGetParams(g, &radius, &length);
renderCylinder(radius, length, position, orientation);
}
else if (type == dCapsuleClass)
{
dReal radius, length;
dGeomCapsuleGetParams(g, &radius, &length);
renderCapsule(radius, length, position, orientation);
}
}
void DynamicsSolver::GetTransform ( dGeomID geom, Matrix* M )
{
float outmatrix[16];
dReal* L; dReal* R;
L = (dReal*)dGeomGetPosition(geom);
R = (dReal*)dGeomGetRotation(geom);
outmatrix[0]=(float)R[0];
outmatrix[1]=(float)R[4];
outmatrix[2]=(float)R[8];
outmatrix[3]=0;
outmatrix[4]=(float)R[1];
outmatrix[5]=(float)R[5];
outmatrix[6]=(float)R[9];
outmatrix[7]=0;
outmatrix[8]=(float)R[2];
outmatrix[9]=(float)R[6];
outmatrix[10]=(float)R[10];
outmatrix[11]=0;
outmatrix[12]=(float)L[0];
outmatrix[13]=(float)L[1];
outmatrix[14]=(float)L[2];
outmatrix[15]=1;
memcpy( M->Mat, outmatrix, sizeof(float) * 16);
}
// Getting data
void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2){
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
dxTriMesh* Geom = (dxTriMesh*)g;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
dVector3 v[3];
FetchTriangle(Geom, Index, Position, Rotation, v);
if (v0){
(*v0)[0] = v[0][0];
(*v0)[1] = v[0][1];
(*v0)[2] = v[0][2];
(*v0)[3] = v[0][3];
}
if (v1){
(*v1)[0] = v[1][0];
(*v1)[1] = v[1][1];
(*v1)[2] = v[1][2];
(*v1)[3] = v[1][3];
}
if (v2){
(*v2)[0] = v[2][0];
(*v2)[1] = v[2][1];
(*v2)[2] = v[2][2];
(*v2)[3] = v[2][3];
}
}
void _InitCylinderTrimeshData(sData& cData)
{
// get cylinder information
// Rotation
const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
dMatrix3Copy(pRotCyc,cData.mCylinderRot);
dGeomGetQuaternion(cData.gCylinder,cData.qCylinderRot);
// Position
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
dVector3Copy(*pPosCyc,cData.vCylinderPos);
// Cylinder axis
dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
// get cylinder radius and size
dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
// get trimesh position and orientation
const dReal* pRotTris = dGeomGetRotation(cData.gTrimesh);
dMatrix3Copy(pRotTris,cData.mTrimeshRot);
dGeomGetQuaternion(cData.gTrimesh,cData.qTrimeshRot);
// Position
const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(cData.gTrimesh);
dVector3Copy(*pPosTris,cData.vTrimeshPos);
// calculate basic angle for 8-gon
dReal fAngle = M_PI / nCYLINDER_CIRCLE_SEGMENTS;
// calculate angle increment
dReal fAngleIncrement = fAngle*REAL(2.0);
// calculate plane normals
// axis dependant code
for(int i=0; i<nCYLINDER_CIRCLE_SEGMENTS; i++)
{
cData.avCylinderNormals[i][0] = -dCos(fAngle);
cData.avCylinderNormals[i][1] = -dSin(fAngle);
cData.avCylinderNormals[i][2] = REAL(0.0);
fAngle += fAngleIncrement;
}
dSetZero(cData.vBestPoint,4);
// reset best depth
cData.fBestCenter = REAL(0.0);
}
mat3 ComponentPhysicsGeom::getOrientation() const
{
const dReal *r = dGeomGetRotation(geom);
return mat3(vec3((float)r[0], (float)r[1], (float)r[2]),
vec3((float)r[4], (float)r[5], (float)r[6]),
vec3((float)r[8], (float)r[9], (float)r[10]));
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldStep (world,0.05);
//if (!pause) dWorldStepFast (world,0.05, 1);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
/*{
for (int i = 1; i < IndexCount; i++) {
dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]);
}
}*/
{const dReal* Pos = dGeomGetPosition(TriMesh);
const dReal* Rot = dGeomGetRotation(TriMesh);
{for (int i = 0; i < IndexCount / 3; i++){
const dVector3& v0 = Vertices[Indices[i * 3 + 0]];
const dVector3& v1 = Vertices[Indices[i * 3 + 1]];
const dVector3& v2 = Vertices[Indices[i * 3 + 2]];
dsDrawTriangle(Pos, Rot, (dReal*)&v0, (dReal*)&v1, (dReal*)&v2, 0);
}}}
if (Ray){
dVector3 Origin, Direction;
dGeomRayGet(Ray, Origin, Direction);
dReal Length = dGeomRayGetLength(Ray);
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);
}
}
void TSRODERigidBody::DebugRender()
{
Graphics()->SetRasterizerState( Graphics()->m_FillWireFrameState );
TSRMatrix4 bodyTransform;
TSRMatrix4 geomTransform;
const float* pBodyPosition = dBodyGetPosition( m_BodyID );
const float* pBodyRotation = dBodyGetRotation( m_BodyID );
ODEToMatrix4( bodyTransform, pBodyPosition, pBodyRotation );
TSRGlobalConstants.PushMatrix();
TSRGlobalConstants.MultMatrix( bodyTransform.d );
TSRDebugDraw::RenderAxis( 1.0f );
TSRDebugDraw::RenderSphere( 0.25f );
for ( unsigned int i = 0; i < m_GeomIDs.size(); i++ )
{
dGeomID currGeomTransformID = m_GeomIDs[ i ];
dGeomID geomID = dGeomTransformGetGeom( currGeomTransformID );
const float* pGeomPosition = dGeomGetPosition( geomID );
const float* pGeomRotation = dGeomGetRotation( geomID );
ODEToMatrix4( bodyTransform, pGeomPosition, pGeomRotation );
TSRGlobalConstants.PushMatrix();
TSRGlobalConstants.MultMatrix( bodyTransform.d );
switch( dGeomGetClass( geomID ) )
{
case dBoxClass:
{
dVector3 extents;
dGeomBoxGetLengths( geomID, extents );
TSRVector3 aabbMin( -extents[ 0 ], -extents[ 1 ], -extents[ 2 ] );
TSRVector3 aabbMax( +extents[ 0 ], +extents[ 1 ], +extents[ 2 ] );
aabbMin *= 0.5f;
aabbMax *= 0.5f;
TSRDebugDraw::RenderAABB( aabbMin, aabbMax );
}
break;
case dSphereClass:
{
float radius;
radius = dGeomSphereGetRadius( geomID );
TSRDebugDraw::RenderSphere( radius );
}
break;
case dCylinderClass:
{
float radius,length;
dGeomCylinderGetParams( geomID, &radius, &length );
TSRDebugDraw::RenderCylinder( length, radius, TSRVector3( 0.0f, 0.0f, 1.0f ) );
}
break;
}
TSRGlobalConstants.PopMatrix();
}
TSRGlobalConstants.PopMatrix();
Graphics()->SetRasterizerState( Graphics()->m_FillSolidState );
}
Pose Primitive::getPose() const {
if(!geom) {
if (!body)
return Pose::translate(0.0f,0.0f,0.0f); // fixes init bug
else
return osgPose(dBodyGetPosition(body), dBodyGetRotation(body));
}
return osgPose(dGeomGetPosition(geom), dGeomGetRotation(geom));
}
void CODEGeom::get_global_center_bt(Fvector& center)
{
center.set(*((const Fvector*)dGeomGetPosition(m_geom_transform)));
dVector3 add;
dMULTIPLY0_331 (add,dGeomGetRotation(m_geom_transform),dGeomGetPosition(geom()));
center.x += add[0];
center.y += add[1];
center.z += add[2];
}
void CBoxGeom::get_max_area_dir_bt(Fvector& dir)
{
dVector3 length,ddir;
dGeomBoxGetLengths (geometry(),length);
dReal S1=length[0]*length[1],S2=length[0]*length[2],S3=length[1]*length[2];
const dReal* R= dGeomGetRotation(geometry());
if(S1>S2)
{
if(S1>S3)
{
ddir[0]=R[2];ddir[1]=R[6];ddir[2]=R[10];//S1
}
else
{
ddir[0]=R[0];ddir[1]=R[4];ddir[2]=R[8];//S3
}
}
else
{
if(S2>S3)
{
ddir[0]=R[1];ddir[1]=R[5];ddir[2]=R[9];//S2
}
else
{
ddir[0]=R[0];ddir[1]=R[4];ddir[2]=R[8];//S3
}
}
if(geom())
{
const dReal* TR=dGeomGetRotation(geometry_transform());
dir.x=dDOT(ddir,TR);
dir.y=dDOT(ddir,TR+4);
dir.z=dDOT(ddir,TR+8);
}
else
{
dir.x=ddir[0];
dir.y=ddir[1];
dir.z=ddir[2];
}
}
void TransformedGeometryExtensionLocalParams(dGeomID geom_transform,const dReal* axis,float center_prg,dReal* local_axis,dReal& local_center_prg)
{
R_ASSERT2(dGeomGetClass(geom_transform)==dGeomTransformClass,"is not a geom transform");
const dReal* rot=dGeomGetRotation(geom_transform);
const dReal* pos=dGeomGetPosition(geom_transform);
dVector3 local_pos;
dMULTIPLY1_331(local_axis,rot,axis);
dMULTIPLY1_331(local_pos,rot,pos);
local_center_prg=center_prg-dDOT(local_pos,local_axis);
}
void CRigidBox::draw() const {
const dReal* v = dGeomGetPosition(mGeomID);
CVector3 o(TO_WORLD(v[0]), TO_WORLD(v[1]), TO_WORLD(v[2]));
const dReal* r = dGeomGetRotation(mGeomID);
F32 sides[3];
dGeomBoxGetLengths(mGeomID, sides);
sides[0] = TO_WORLD(sides[0]);
sides[1] = TO_WORLD(sides[1]);
sides[2] = TO_WORLD(sides[2]);
drawBox(&o.x, r, sides);
}
void SkidSteeringVehicle::draw() {
{
dsSetColor(0, 0, 1);
const dReal *pos = dGeomGetPosition(this->vehicleGeom);
const dReal *R = dGeomGetRotation(this->vehicleGeom);
dReal sides[3];
dGeomBoxGetLengths(this->vehicleGeom, sides);
dsDrawBoxD(pos, R, sides);
}
dsSetColor(1, 1, 0);
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
const dReal *pos = dGeomGetPosition(this->wheelGeom[fr][lr]);
const dReal *R = dGeomGetRotation(this->wheelGeom[fr][lr]);
dReal radius, length;
dGeomCylinderGetParams(this->wheelGeom[fr][lr], &radius, &length);
dsDrawCylinderD(pos, R, length, radius);
}
}
}
void CODEGeom::get_final_tx(dGeomID g,const dReal* &p,const dReal* &R,dReal * bufV, dReal* bufM)
{
if(is_transform(g))
{
computeFinalTx(g,bufV,bufM);
R=bufM;p=bufV;
}else
{
R=dGeomGetRotation(g);
p=dGeomGetPosition(g);
}
}
/*** 車輪の描画 ***/
void drawWheel()
{
dReal radius, length;
dsSetColor(1.1,1.1,1.1);
for (int i=0; i< WHEEL_NUM; i++)
{
dGeomCylinderGetParams(wheel[i].geom, &radius, &length);
dsDrawCylinder(dGeomGetPosition(wheel[i].geom),
dGeomGetRotation(wheel[i].geom),length, radius);
}
}
void sTrimeshCapsuleColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom *Capsule,
int flags, int skip)
{
const dMatrix3* pRot = (const dMatrix3*)dGeomGetRotation(Capsule);
memcpy(m_mCapsuleRotation, pRot, sizeof(dMatrix3));
const dVector3* pDst = (const dVector3*)dGeomGetPosition(Capsule);
memcpy(m_vCapsulePosition, pDst, sizeof(dVector3));
m_vCapsuleAxis[0] = m_mCapsuleRotation[0*4 + nCAPSULE_AXIS];
m_vCapsuleAxis[1] = m_mCapsuleRotation[1*4 + nCAPSULE_AXIS];
m_vCapsuleAxis[2] = m_mCapsuleRotation[2*4 + nCAPSULE_AXIS];
// Get size of Capsule
dGeomCapsuleGetParams(Capsule, &m_vCapsuleRadius, &m_fCapsuleSize);
m_fCapsuleSize += 2*m_vCapsuleRadius;
const dMatrix3* pTriRot = (const dMatrix3*)dGeomGetRotation(TriMesh);
memcpy(m_mTriMeshRot, pTriRot, sizeof(dMatrix3));
const dVector3* pTriPos = (const dVector3*)dGeomGetPosition(TriMesh);
memcpy(m_mTriMeshPos, pTriPos, sizeof(dVector3));
// global info for contact creation
m_iStride =skip;
m_iFlags =flags;
// reset contact counter
m_ctContacts = 0;
// reset best depth
m_fBestDepth = - MAX_REAL;
m_fBestCenter = 0;
m_fBestrt = 0;
// reset collision normal
m_vNormal[0] = REAL(0.0);
m_vNormal[1] = REAL(0.0);
m_vNormal[2] = REAL(0.0);
}
static void simLoop (int pause)
{
double simstep = 0.001; // 1ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf(dt/simstep);
// fprintf(stderr, "dt=%f, nr of steps = %d\n", dt, nrofsteps);
for (int i=0; i<nrofsteps && !pause; i++)
{
dSpaceCollide (space,0,&nearCallback);
dWorldQuickStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
dsSetColor (1,1,1);
const dReal *SPos = dBodyGetPosition(sphbody);
const dReal *SRot = dBodyGetRotation(sphbody);
float spos[3] = {SPos[0], SPos[1], SPos[2]};
float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] };
dsDrawSphere
(
spos,
srot,
RADIUS
);
// draw world trimesh
dsSetColor(0.4,0.7,0.9);
dsSetTexture (DS_NONE);
const dReal* Pos = dGeomGetPosition(world_mesh);
//dIASSERT(dVALIDVEC3(Pos));
float pos[3] = { Pos[0], Pos[1], Pos[2] };
const dReal* Rot = dGeomGetRotation(world_mesh);
//dIASSERT(dVALIDMAT3(Rot));
float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] };
int numi = sizeof(world_indices) / sizeof(int);
for (int i=0; i<numi/3; i++)
{
int i0 = world_indices[i*3+0];
int i1 = world_indices[i*3+1];
int i2 = world_indices[i*3+2];
float *v0 = world_vertices+i0*3;
float *v1 = world_vertices+i1*3;
float *v2 = world_vertices+i2*3;
dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
}
}
static void drawBox (dGeomID id, int R, int G, int B)
{
if (!id)
return;
const dReal *pos = dGeomGetPosition (id);
const dReal *rot = dGeomGetRotation (id);
dsSetColor (R,G,B);
dVector3 l;
dGeomBoxGetLengths (id, l);
dsDrawBox (pos, rot, l);
}
void Solid::prepare()
{
memcpy(m_last_transform,m_current_transform,16) ;
dGeomTriMeshSetLastTransform(m_geometry1,m_last_transform) ;
const dReal* Pos = dGeomGetPosition(m_geometry1);
const dReal* Rot = dGeomGetRotation(m_geometry1);
m_current_transform[ 0 ] = Rot[ 0 ]; m_current_transform[ 1 ] = Rot[ 1 ]; m_current_transform[ 2 ] = Rot[ 2 ]; m_current_transform[ 3 ] = 0;
m_current_transform[ 4 ] = Rot[ 4 ]; m_current_transform[ 5 ] = Rot[ 5 ]; m_current_transform[ 6 ] = Rot[ 6 ]; m_current_transform[ 7 ] = 0;
m_current_transform[ 8 ] = Rot[ 8 ]; m_current_transform[ 9 ] = Rot[ 9 ]; m_current_transform[10 ] = Rot[10 ]; m_current_transform[11 ] = 0;
m_current_transform[12 ] = Pos[ 0 ]; m_current_transform[13 ] = Pos[ 1 ]; m_current_transform[14 ] = Pos[ 2 ]; m_current_transform[15 ] = 1;
}
