int main (int argc, char **argv)
{
  // 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_TEXTURE_PATH;
  if(argc==2)
    {
        fn.path_to_textures = argv[1];
    }

  // create world
  dInitODE2(0);
  world = dWorldCreate();
 
  space = dSimpleSpaceCreate(0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-0.5);
  dWorldSetCFM (world,1e-5);
  dCreatePlane (space,0,0,1,0);
  memset (obj,0,sizeof(obj));

  // note: can't share tridata if intending to trimesh-trimesh collide
  TriData1 = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
  TriData2 = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
  
  TriMesh1 = dCreateTriMesh(space, TriData1, 0, 0, 0);
  TriMesh2 = dCreateTriMesh(space, TriData2, 0, 0, 0);
  dGeomSetData(TriMesh1, TriData1);
  dGeomSetData(TriMesh2, TriData2);
  
  {dGeomSetPosition(TriMesh1, 0, 0, 0.9);
  dMatrix3 Rotation;
  dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
  dGeomSetRotation(TriMesh1, Rotation);}

  {dGeomSetPosition(TriMesh2, 1, 0, 0.9);
  dMatrix3 Rotation;
  dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
  dGeomSetRotation(TriMesh2, Rotation);}
  
  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}
示例#2
0
int test_dBoxTouchesBox()
{
  int k,bt1,bt2;
  dVector3 p1,p2,side1,side2;
  dMatrix3 R1,R2;

  dSimpleSpace space(0);
  dGeomID box1 = dCreateBox (0,1,1,1);
  dSpaceAdd (space,box1);
  dGeomID box2 = dCreateBox (0,1,1,1);
  dSpaceAdd (space,box2);

  dMakeRandomVector (p1,3,0.5);
  dMakeRandomVector (p2,3,0.5);
  for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
  for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
  dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
		      dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
  dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
		      dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);

  dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
  dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
  dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
  dGeomSetRotation (box1,R1);
  dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
  dGeomSetRotation (box2,R2);
  draw_all_objects (space);

  int t1 = testBoxesTouch2 (p1,R1,side1,p2,R2,side2);
  int t2 = testBoxesTouch2 (p2,R2,side2,p1,R1,side1);
  bt1 = t1 || t2;
  bt2 = dBoxTouchesBox (p1,R1,side1,p2,R2,side2);

  if (bt1 != bt2) FAILED();

  /*
    // some more debugging info if necessary
    if (bt1 && bt2) printf ("agree - boxes touch\n");
    if (!bt1 && !bt2) printf ("agree - boxes don't touch\n");
    if (bt1 && !bt2) printf ("disagree - boxes touch but dBoxTouchesBox "
			     "says no\n");
    if (!bt1 && bt2) printf ("disagree - boxes don't touch but dBoxTouchesBox "
			     "says yes\n");
  */

  PASSED();
}
示例#3
0
void command (int cmd)
{
  // note: 0.0174532925 radians = 1 degree
  dQuaternion q;
  dMatrix3 m;
  switch(cmd)
    {
    case 'w':
      geom1pos[0]+=0.05;
      break;
    case 'a':
      geom1pos[1]-=0.05;
      break;
    case 's':
      geom1pos[0]-=0.05;
      break;
    case 'd':
      geom1pos[1]+=0.05;
      break;
    case 'e':
      geom1pos[2]-=0.05;
      break;
    case 'q':
      geom1pos[2]+=0.05;
      break;
    case 'i':
      dQFromAxisAndAngle (q, 0, 0, 1,0.0174532925);
      dQMultiply0(geom1quat,geom1quat,q);
      break;
    case 'j':
      dQFromAxisAndAngle (q, 1, 0, 0,0.0174532925);
      dQMultiply0(geom1quat,geom1quat,q);
      break;
    case 'k':
      dQFromAxisAndAngle (q, 0, 0, 1,-0.0174532925);
      dQMultiply0(geom1quat,geom1quat,q);
      break;
    case 'l':
      dQFromAxisAndAngle (q, 1, 0, 0,-0.0174532925);
      dQMultiply0(geom1quat,geom1quat,q);
      break;
    case 'm':
		(drawmode!=DS_POLYFILL)? drawmode=DS_POLYFILL:drawmode=DS_WIREFRAME;
      break;
    case 'n':
		(geoms!=convex)? geoms=convex:geoms=boxes;
      break;
    default:
      dsPrint ("received command %d (`%c')\n",cmd,cmd);     
    }
#if 0
  dGeomSetPosition (geoms[1],
		    geom1pos[0],
		    geom1pos[1],
		    geom1pos[2]);
  dQtoR (geom1quat, m);
  dGeomSetRotation (geoms[1],m);
#endif
  DumpInfo=true;
}
示例#4
0
void Obstacle::on_addToScene()
{
    node_ = SceneGraph::addModel(name_, model_);
    size_t batchCnt = 0;
    TriangleBatch const *triBatch = model_->batches(&batchCnt);
    size_t boneCnt = 0;
    Bone const *bone = model_->bones(&boneCnt);
    size_t vSize = model_->vertexSize();
    char const * vertex = (char const *)model_->vertices();
    unsigned int const *index = (unsigned int const *)model_->indices();
    for (size_t bi = 0; bi != batchCnt; ++bi)
    {
        dTriMeshDataID tmd = dGeomTriMeshDataCreate();
        dGeomTriMeshDataBuildSingle(tmd, vertex, vSize, triBatch[bi].maxVertexIndex + 1, 
            index + triBatch[bi].firstTriangle * 3, triBatch[bi].numTriangles * 3, 12);
        dGeomID geom = dCreateTriMesh(gStaticSpace, tmd, 0, 0, 0);
        tmd_.push_back(tmd);
        geom_.push_back(geom);
        Matrix bx;
        get_bone_transform(bone, triBatch[bi].bone, bx);
        Vec3 p(bx.translation());
        addTo(p, pos());
        dGeomSetPosition(geom, p.x, p.y, p.z);
        dGeomSetRotation(geom, bx.rows[0]);
    }
}
void Compound::addGeometry(const Pose3<>& parentPose, Geometry& geometry, SimRobotCore2::CollisionCallback* callback)
{
  // compute pose
  Pose3<> geomPose = parentPose;
  if(geometry.translation)
    geomPose.translate(*geometry.translation);
  if(geometry.rotation)
    geomPose.rotate(*geometry.rotation);

  // create geometry
  dGeomID geom = geometry.createGeometry(Simulation::simulation->staticSpace);
  if(geom)
  {
    dGeomSetData(geom, &geometry);

    // set pose
    dGeomSetPosition(geom, geomPose.translation.x, geomPose.translation.y, geomPose.translation.z);
    dMatrix3 matrix3;
    ODETools::convertMatrix(geomPose.rotation, matrix3);
    dGeomSetRotation(geom, matrix3);
  }

  // handle nested geometries
  for(std::list< ::PhysicalObject*>::const_iterator iter = geometry.physicalDrawings.begin(), end = geometry.physicalDrawings.end(); iter != end; ++iter)
  {
    Geometry* geometry = dynamic_cast<Geometry*>(*iter);
    if(geometry)
      addGeometry(geomPose, *geometry, callback);
  }
}
//===========================================================================
void cODEGenericBody::setRotation(cMatrix3d &a_rotation)
{
    // apply new rotation to ODE body
	dMatrix3 R;

	R[0]  = a_rotation.m[0][0];
	R[1]  = a_rotation.m[0][1];
	R[2]  = a_rotation.m[0][2];
	R[4]  = a_rotation.m[1][0];
	R[5]  = a_rotation.m[1][1];
	R[6]  = a_rotation.m[1][2];
	R[8]  = a_rotation.m[2][0];
	R[9]  = a_rotation.m[2][1];
	R[10] = a_rotation.m[2][2];

    // check if body defined
    if (m_ode_body != NULL)
    {
        // store new rotation matrix
        m_localRot = a_rotation;
        dBodySetRotation(m_ode_body, R);
    }
    else if (m_ode_geom != NULL)
    {
        // store new rotation matrix
        m_localRot = a_rotation;
        dGeomSetRotation(m_ode_geom, R);
    }
}
void CProtoHapticDoc::UpdateDynamics()
{
	for(int i= 0; i<m_shapeCount; i++) {

		dGeomBoxSetLengths (m_geoms[i], m_shapes[i]->getSizeX(),
									    m_shapes[i]->getSizeY(),
									    m_shapes[i]->getSizeZ());

		dGeomSetPosition   (m_geoms[i], m_shapes[i]->getLocationX(),
									    m_shapes[i]->getLocationY(),
									    m_shapes[i]->getLocationZ());
		dGeomSetRotation   (m_geoms[i], dBodyGetRotation(bodies[i]));

		dBodySetPosition   (bodies[i], m_shapes[i]->getLocationX(),
									   m_shapes[i]->getLocationY(),
									   m_shapes[i]->getLocationZ());

		float *rotation= m_shapes[i]->getRotation();

		const dReal rot[12]=
		{ rotation[0], rotation[4], rotation[8], rotation[12],
		  rotation[1], rotation[5], rotation[9], rotation[13],
		  rotation[2], rotation[6], rotation[10], rotation[14] };

		dBodySetRotation   (bodies[i], rot);

		dMass mass;
		dMassSetBox (&mass, m_shapes[i]->getMass(),m_shapes[i]->getSizeX(),
								                   m_shapes[i]->getSizeY(),
								                   m_shapes[i]->getSizeZ());

		dBodySetMass (bodies[i], &mass);

	}
}
void ApproxDistanceSensor::DistanceSensor::updateValue()
{
  pose = physicalObject->pose;
  pose.conc(offset);
  invertedPose = pose.invert();
  Vector3<> boxPos = pose * Vector3<>(max * 0.5f, 0.f, 0.f);
  dGeomSetPosition(geom, boxPos.x, boxPos.y, boxPos.z);
  dMatrix3 matrix3;
  ODETools::convertMatrix(pose.rotation, matrix3);
  dGeomSetRotation(geom, matrix3);
  closestGeom = 0;
  closestSqrDistance = maxSqrDist;
  dSpaceCollide2(geom, (dGeomID)Simulation::simulation->movableSpace, this, (dNearCallback*)&staticCollisionWithSpaceCallback);
  dSpaceCollide2(geom, (dGeomID)Simulation::simulation->staticSpace, this, (dNearCallback*)&staticCollisionCallback);
  if(closestGeom)
  {
    const dReal* pos = dGeomGetPosition(closestGeom);
    Geometry* geometry = (Geometry*)dGeomGetData(closestGeom);
    data.floatValue = (Vector3<>((float) pos[0], (float) pos[1], (float) pos[2]) - pose.translation).abs() - geometry->innerRadius;
    if(data.floatValue < min)
      data.floatValue = min;
  }
  else
    data.floatValue = max;
}
示例#9
0
// Universal method for all specific ODE geom types, which add the
// geom to the collide space, using an ODE proxy geom to offset the
// geom by the provided transformation matrix. The geom will also
// be attached to the rigid body, if any is set.
void CShape::AttachGeom(dGeomID GeomId, dSpaceID SpaceID)
{
	n_assert(GeomId);
	n_assert(!IsAttached());

	// set the geom's local Transform
	const vector3& Pos = Transform.pos_component();
	dGeomSetPosition(GeomId, Pos.x, Pos.y, Pos.z);
	dMatrix3 ODERotation;
	CPhysicsServer::Matrix44ToOde(Transform, ODERotation);
	dGeomSetRotation(GeomId, ODERotation);

	// if attached to rigid body, create a geom Transform "proxy" object && attach it to the rigid body
	// else directly set Transform and rotation
	if (pRigidBody)
	{
		ODEGeomID = dCreateGeomTransform(0);
		dGeomTransformSetCleanup(ODEGeomID, 1);
		dGeomTransformSetGeom(ODEGeomID, GeomId);
		dGeomSetBody(ODEGeomID, pRigidBody->GetODEBodyID());
	}
	else ODEGeomID = GeomId;

	dGeomSetCategoryBits(ODEGeomID, CatBits);
	dGeomSetCollideBits(ODEGeomID, CollBits);
	dGeomSetData(ODEGeomID, this);
	AttachToSpace(SpaceID);
}
示例#10
0
void TSRODERigidBody::AddCylinderGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, float _fRadius,float _fLength, float _fDensity )
{
	TSRODEPhysicsWorld* _pWorld = ( TSRODEPhysicsWorld* ) _pWorldInterface;
    dMass totalMass;
    dBodyGetMass( m_BodyID, &totalMass );
    if ( m_GeomIDs.size() == 0 )
    {
        dMassSetZero( &totalMass );
    }
    dMatrix4 R;
    dVector3 P;
    Matrix4ToODE( _bodyToGeomTransform, R, P );
    dGeomID geomTransform = dCreateGeomTransform( _pWorld->m_SpaceID );
    dGeomID encapsulatedGeom = 0;
    dMass currMass;
    dMassSetZero( &currMass );

    encapsulatedGeom = dCreateCylinder( 0, _fRadius, _fLength );

    dMassSetCylinder( &currMass, _fDensity, 0, _fRadius, _fLength );
    dMassRotate( &currMass, R );
    //dMassTranslate(&currMass,P[0],P[1],P[2]);
    dMassAdd( &totalMass, &currMass );
    dGeomSetPosition( encapsulatedGeom, P[ 0 ], P[ 1 ], P[ 2 ] );
    dGeomSetRotation( encapsulatedGeom, R );
    dGeomTransformSetCleanup( geomTransform, 1 );
    dGeomTransformSetGeom( geomTransform, encapsulatedGeom );
    dGeomSetBody( geomTransform, m_BodyID );

    m_GeomIDs.push_back( geomTransform );
    dBodySetMass( m_BodyID, &totalMass );

}
示例#11
0
    void cPhysicsObject::InitCommon(cWorld* pWorld, const physvec_t& posOriginal, const physvec_t& rot)
    {
      math::cVec3 pos(posOriginal.x, posOriginal.y, posOriginal.z + fHeight);

      rotation.LoadIdentity();
      rotation.SetFromAngles(math::DegreesToRadians(rot));

      const math::cMat4 m = rotation.GetMatrix();

      dMatrix3 r;
      r[0] = m[0];    r[1] = m[4];    r[2] = m[8];    r[3] = 0;
      r[4] = m[1];    r[5] = m[5];    r[6] = m[9];    r[7] = 0;
      r[8] = m[2];    r[9] = m[6];    r[10] = m[10];  r[11] = 0;

      position = pos;

      dGeomSetPosition(geom, position.x, position.y, position.z);
      dGeomSetRotation(geom, r);

      if (bBody) {
        body = dBodyCreate(pWorld->GetWorld());
        dBodySetPosition(body, position.x, position.y, position.z);
        dBodySetRotation(body, r);
        dBodySetAutoDisableFlag(body, 1);

        dGeomSetBody(geom, body);

        pWorld->AddPhysicsObject(shared_from_this());
      }
    }
//===========================================================================
void cODEGenericBody::createDynamicBox(const double a_lengthX,
									   const double a_lengthY,
									   const double a_lengthZ,
                                       bool a_staticObject,
									   const cVector3d& a_offsetPos,
									   const cMatrix3d& a_offsetRot)
{
    // create ode dynamic body if object is non static
    if (!a_staticObject)
    {
        m_ode_body = dBodyCreate(m_ODEWorld->m_ode_world);

	    // store pointer to current object
	    dBodySetData (m_ode_body, this);
    }
    m_static = a_staticObject;

    // build box
    m_ode_geom = dCreateBox(m_ODEWorld->m_ode_space, a_lengthX, a_lengthY, a_lengthZ);

	// adjust position offset
	dGeomSetPosition (m_ode_geom, a_offsetPos.x, a_offsetPos.y, a_offsetPos.z);

	// adjust orientation offset
	dMatrix3 R;
	R[0]  = a_offsetRot.m[0][0];
	R[1]  = a_offsetRot.m[0][1];
	R[2]  = a_offsetRot.m[0][2];
	R[4]  = a_offsetRot.m[1][0];
	R[5]  = a_offsetRot.m[1][1];
	R[6]  = a_offsetRot.m[1][2];
	R[8]  = a_offsetRot.m[2][0];
	R[9]  = a_offsetRot.m[2][1];
	R[10] = a_offsetRot.m[2][2];
	dGeomSetRotation (m_ode_geom, R);

    // set inertia properties
    if (!m_static)
    {
        dMassSetBox(&m_ode_mass, 1.0, a_lengthX, a_lengthY, a_lengthZ);
	    dMassAdjust(&m_ode_mass, m_mass);
	    dBodySetMass(m_ode_body,&m_ode_mass);

        // attach body and geometry together
        dGeomSetBody(m_ode_geom, m_ode_body);
    }

    // store dynamic model type
    m_typeDynamicCollisionModel = ODE_MODEL_BOX;

    // store dynamic model parameters
    m_paramDynColModel0 = a_lengthX;
    m_paramDynColModel1 = a_lengthY;
    m_paramDynColModel2 = a_lengthZ;
    m_posOffsetDynColModel = a_offsetPos;
    m_rotOffsetDynColModel = a_offsetRot;
}
示例#13
0
void CODEGeom::set_static_ref_form(const Fmatrix& form)
{
	dGeomSetPosition(geometry_transform(),form.c.x,form.c.y,form.c.z);
	Fmatrix33 m33;
	m33.set(form);
	dMatrix3 R;
	PHDynamicData::FMX33toDMX(m33,R);
	dGeomSetRotation(geometry_transform(),R);
}
示例#14
0
文件: dm6.cpp 项目: Ry0/ODE
void dmCreateBox0(dmObject *obj, double p[3], double R[12], double m, double side[3], double color[3])
{
	obj->body = NULL;
	obj->side = side;
	obj->color = color;
	obj->p = p;
	obj->R = R;

	obj->geom = dCreateBox(space,obj->side[0], obj->side[1], obj->side[2]); // ボックスジオメトリの生成
	dGeomSetPosition(obj->geom, obj->p[0], obj->p[1], obj->p[2]);  // ボールの位置(x,y,z)を設定
	dGeomSetRotation(obj->geom, obj->R);
}
示例#15
0
文件: dm6.cpp 项目: Ry0/ODE
void dmCreateSphere0(dmObject *obj, double p[2], double R[12], double m, double r, double color[3])
{
	obj->body = NULL;
	obj->m = m;
	obj->r = r;
	obj->R = R;
	obj->p = p;
	obj->color = color;

	obj->geom = dCreateSphere(space,obj->r); // 球ジオメトリの生成
	dGeomSetPosition(obj->geom, obj->p[0], obj->p[1], obj->p[2]);  // ボールの位置(x,y,z)を設定
	dGeomSetRotation(obj->geom, obj->R);
}
示例#16
0
void Object::setRotation(float x,float y,float z)
{
	dMatrix3 R,R0,R1,R2,R3;

	dRFromAxisAndAngle(R1,1,0,0,DEG2RAD(x));
	dRFromAxisAndAngle(R2,0,1,0,DEG2RAD(y));
	dRFromAxisAndAngle(R3,0,0,1,DEG2RAD(z));
	dMultiply0 (R0,R1,R2,3,3,3);
	dMultiply0 (R, R0,R3,3,3,3);

	dGeomSetRotation(iGeom,R);
	dBodySetRotation(iBody,R);
}
示例#17
0
文件: dm6.cpp 项目: Ry0/ODE
void dmCreateCapsule0(dmObject *obj, double p[3], double R[12], double m, double r, double l, double color[3])
{
	obj->body = NULL;
	obj->r = r;
	obj->l = l;
	obj->color = color;
	obj->p = p;
	obj->R = R;

	obj->geom = dCreateCapsule(space,obj->r, obj->l); // 円柱ジオメトリの生成
	dGeomSetPosition(obj->geom, obj->p[0], obj->p[1], obj->p[2]);  // ボールの位置(x,y,z)を設定
	dGeomSetRotation(obj->geom, obj->R);
}
示例#18
0
int CubeBasePiece::selectFace(int& numFaces, dSpaceID space, dGeomID ray, float cameraX, float cameraY, float cameraZ)
{
    numFaces = 6;

    const dReal* rotationMatrix = dGeomGetRotation(geom);
    const dReal* currentPosition = dGeomGetPosition(geom);

    dGeomID faces[6];

    // create six faces
    faces[0] = dCreateBox(space,.01,sides[1],sides[2]);
    faces[1] = dCreateBox(space,.01,sides[1],sides[2]);
    dGeomSetPosition(faces[0],currentPosition[0]-(sides[0]/2),currentPosition[1],currentPosition[2]);
    dGeomSetPosition(faces[1],currentPosition[0]+(sides[0]/2),currentPosition[1],currentPosition[2]);
    faces[2] = dCreateBox(space,sides[0],.01,sides[2]);
    faces[3] = dCreateBox(space,sides[0],.01,sides[2]);
    dGeomSetPosition(faces[2],currentPosition[0],currentPosition[1]+(sides[1]/2),currentPosition[2]);
    dGeomSetPosition(faces[3],currentPosition[0],currentPosition[1]-(sides[1]/2),currentPosition[2]);
    faces[4] = dCreateBox(space,sides[0],sides[1],.01);
    faces[5] = dCreateBox(space,sides[0],sides[1],.01);
    dGeomSetPosition(faces[4],currentPosition[0],currentPosition[1],currentPosition[2]+(sides[2]/2));
    dGeomSetPosition(faces[5],currentPosition[0],currentPosition[1],currentPosition[2]-(sides[2]/2));

    int selectedFace;
    int selectedPointDistance = 1000;
    dContactGeom contact[1];


    // find collision point closest to camera
    for(int i = 0 ; i < 6 ; i++)
    {
        // rotate geom before collision
        dGeomSetRotation(faces[i],rotationMatrix);

        if(dCollide(faces[i],ray,1,&contact[0],sizeof(dContact)) != 0)
        {
            float tempDistance = pow(contact[0].pos[0] - cameraX,2) + pow(contact[0].pos[1] - cameraY,2) + pow(contact[0].pos[2] - cameraZ,2);
            if(tempDistance <= selectedPointDistance)
            {
                selectedPointDistance = tempDistance;
                selectedFace = i;
            }
        }

        dGeomDestroy(faces[i]); // delete the geoms
    }

    return selectedFace;
}
示例#19
0
void CShape::SetTransform(const matrix44& Tfm)
{
	Transform = Tfm;

	// if not attached to rigid body, directly update pShape's transformation
	if (!pRigidBody && ODEGeomID)
	{
		//!!!DUPLICATE CODE!
		const vector3& Pos = Transform.pos_component();
		dGeomSetPosition(ODEGeomID, Pos.x, Pos.y, Pos.z);
		dMatrix3 ODERotation;
		CPhysicsServer::Matrix44ToOde(Transform, ODERotation);
		dGeomSetRotation(ODEGeomID, ODERotation);
	}
}
示例#20
0
void Object::MakeGeom(dSpaceID space)
{
	iGeom = dCreateBox(space,iSize.x,iSize.y,iSize.z);

	dMatrix3 R,R0,R1,R2,R3;

	dRFromAxisAndAngle(R1,1,0,0,DEG2RAD(iRotate.x));
	dRFromAxisAndAngle(R2,0,1,0,DEG2RAD(iRotate.y));
	dRFromAxisAndAngle(R3,0,0,1,DEG2RAD(iRotate.z));
	dMultiply0 (R0,R1,R2,3,3,3);
	dMultiply0 (R, R0,R3,3,3,3);

	dGeomSetRotation(iGeom,R);
	dGeomSetPosition(iGeom, iPosition.x, iPosition.y, iPosition.z);
};
示例#21
0
void CappedCylinder::createGeometry(dSpaceID space)
{
  if(geometry == 0)
  {
    setGeometry(dCreateCCylinder(space, dReal(radius), dReal(height)));
    dGeomSetPosition(geometry, dReal(position.v[0]), dReal(position.v[1]), dReal(position.v[2]));
    dMatrix3 rotationMatrix;
    ODETools::convertMatrix(rotation, rotationMatrix);
    dGeomSetRotation(geometry, rotationMatrix);
    //set user data pointer of ODE geometry object to this physical object
    dGeomSetData(geometry, this);
    //set collide bitfield
    PhysicalObject::setCollideBitfield();
  }
}
示例#22
0
void ComponentPhysicsGeom::setOrientation(const mat3 &m)
{
	dMatrix3 r;

	const vec3 x = m.getAxisX().getNormal();
	const vec3 y = m.getAxisY().getNormal();
	const vec3 z = m.getAxisZ().getNormal();

	r[0] = x.x; r[1] = x.y; r[2] = x.z; r[3] = 0.0f;
	r[4] = y.x; r[5] = y.y; r[6] = y.z; r[7] = 0.0f;
	r[8] = z.x; r[9] = z.y; r[10]= z.z; r[11]= 0.0f;

	dGeomSetRotation(geom, r);

	getParentBlackBoard().relayMessage(MessageOrientationHasBeenSet(getOrientation()));
}
示例#23
0
void StaticWorldObject::AddLeaf(ssgLeaf *leaf, sgVec3 initialpos)
{
  // traverse the triangles
  int cnt = leaf->getNumTriangles() ;
  int nv  = leaf->getNumVertices() ;
//  int nn  = leaf->getNumNormals() ;

  float *vertices = new float[3*nv];
  int   *indices  = new int[3*cnt];

  int i;
  for (i=0; i<nv; i++)
  {
    float *v = leaf->getVertex( i ) ;
    assert(v);
    memcpy(vertices+3*i, v, 3*sizeof(float));
  }
  for (i=0; i<cnt; i++)
  {
    short idx0, idx1, idx2 ;
    leaf->getTriangle( i, &idx0, &idx1, &idx2 ) ;
    indices[3*i+0]=idx0;
    indices[3*i+1]=idx1;
    indices[3*i+2]=idx2;
  }

  dTriMeshDataID data = dGeomTriMeshDataCreate();
  dataids.push_back(data);
  dGeomTriMeshDataBuildSingle
  (
    data, 
    vertices,
    3*sizeof(float), 
    nv, 
    indices,
    cnt*3, 
    3*sizeof(int)
  );
  //fprintf(stderr,"Adding trimesh with %d verts, %d indices\n", nv, cnt*3);
  dGeomID trimesh = dCreateTriMesh(space, data, 0,0,0);
  geomids.push_back(trimesh);
  dGeomSetPosition(trimesh, initialpos[0], initialpos[1], initialpos[2]);
  dMatrix3 R;
  dRFromAxisAndAngle (R, 0,1,0, 0.0);
  dGeomSetRotation (trimesh, R);
  dGeomSetData(trimesh, this);
}
示例#24
0
void construirQuinas()
{
    // Walls
    // 0 front      1 back
    // 2 right top  3 right back
    // 4 left top   5 left back
    int n[7] = {1, 1, -1, -1, 1, 1, -1};
    dMatrix3 R;

    for(int i=0; i < 4; i++)
    {
        triangle[i] = dCreateBox(space,TRIANGLE_THICK,TRIANGLE_LENGTH,HEIGHT);
        dGeomSetPosition(triangle[i],n[i]*TRIANGLE_X,n[i+1]*TRIANGLE_Y,0);
        dRFromAxisAndAngle (R,0,0,1,n[i*2]*M_PI_4);
        dGeomSetRotation (triangle[i], R);
    }
}
示例#25
0
void CollisionMesh::Finalize()
{
	//create mesh data structure
	meshData = dGeomTriMeshDataCreate();

	//create indices
	int indexlist = new int[TriangleList.size()*3];
	for( int i = 0; i< TriangleList.size()*3; i++)
	{
		indexlist[i] = i;
	}

	//copy over vertex data into buffer
	dReal* vertices = new dReal[TriangleList.size()*9];
	int vi=0;
	for ( int i=0; i<TriangleList.size(); i++ )
	{
		vertices[vi+0] = TriangleList[i].v1.x;
		vertices[vi+1] = TriangleList[i].v1.y;
		vertices[vi+2] = TriangleList[i].v1.z;
		vertices[vi+3] = TriangleList[i].v2.x;
		vertices[vi+4] = TriangleList[i].v2.y;
		vertices[vi+5] = TriangleList[i].v2.z;
		vertices[vi+6] = TriangleList[i].v3.x;
		vertices[vi+7] = TriangleList[i].v3.y;
		vertices[vi+8] = TriangleList[i].v3.z;
		vi += 9;

	}

	dGeomTriMeshDataBuildSingle(meshData, vertices, sizeof(dReal)*3,
		TriangleList.size()*3, 
		(const int*)indexlist, 
		TriangleList.size()*3, 3*sizeof( int ));

	Geom = dCreateTriMesh(solver->GetSpaceID(true, Location.x, Location.y, Location.z), meshData, 0, 0, 0);
	dGeomSetData( Geom, &SurfaceDesc );
	dGeomSetPosition( Geom, Location.x, Location.y, Location.z );
	dMatrix3 R;
	dRFromEulerAngles (R, pitch, -yaw, roll);
	dGeomSetRotation( Geom, R );
	Initialized = true;



}
示例#26
0
void ODEObject::on_set_rotation(void *me, OscMatrix3 &r)
{
    // Convert from a CHAI rotation matrix to an ODE rotation matrix
    dMatrix3 m;
    m[ 0] = r.getCol0().x;
    m[ 1] = r.getCol0().y;
    m[ 2] = r.getCol0().z;
    m[ 3] = 0;
    m[ 4] = r.getCol1().x;
    m[ 5] = r.getCol1().y;
    m[ 6] = r.getCol1().z;
    m[ 7] = 0;
    m[ 8] = r.getCol2().x;
    m[ 9] = r.getCol2().y;
    m[10] = r.getCol2().z;
    m[11] = 0;
    dGeomSetRotation(((ODEObject*)me)->m_odeGeom, m);
}
示例#27
0
void CBoxGeom::set_position(const Fvector& ref_point)
{

	inherited::set_position(ref_point);

	dVector3 local_position={m_box.m_translate.x-ref_point.x,
							m_box.m_translate.y-ref_point.y,
							m_box.m_translate.z-ref_point.z
							};
	dGeomSetPosition(geom(),
					local_position[0],
					local_position[1],
					local_position[2]
					);
	dMatrix3 R;
	PHDynamicData::FMX33toDMX(m_box.m_rotate,R);
	dGeomSetRotation(geom(),R);
}
示例#28
0
int test_sphere_point_depth()
{
  int j;
  dVector3 p,q;
  dMatrix3 R;
  dReal r,d;

  dSimpleSpace space(0);
  dGeomID sphere = dCreateSphere (0,1);
  dSpaceAdd (space,sphere);

  // ********** make a random sphere of radius r at position p

  r = dRandReal()+0.1;
  dGeomSphereSetRadius (sphere,r);
  dMakeRandomVector (p,3,1.0);
  dGeomSetPosition (sphere,p[0],p[1],p[2]);
  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
		      dRandReal()*2-1,dRandReal()*10-5);
  dGeomSetRotation (sphere,R);

  // ********** test center point has depth r

  if (dFabs(dGeomSpherePointDepth (sphere,p[0],p[1],p[2]) - r) > tol) FAILED();

  // ********** test point on surface has depth 0

  for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
  dNormalize3 (q);
  for (j=0; j<3; j++) q[j] = q[j]*r + p[j];
  if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])) > tol) FAILED();

  // ********** test point at random depth

  d = (dRandReal()*2-1) * r;
  for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
  dNormalize3 (q);
  for (j=0; j<3; j++) q[j] = q[j]*(r-d) + p[j];
  if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])-d) > tol) FAILED();

  PASSED();
}
示例#29
0
void HarrierSim::makeHarrier()
{
  dMass mass;
  itsHarrierBody = dBodyCreate(world);
  //pos[0] = 0; pos[1] = 1.0*5; pos[2] = 1.80;

  dBodySetPosition(itsHarrierBody,0,0.2*5,4.94);

  dMatrix3 R;
  dRFromAxisAndAngle (R,1,0,0,0);
  dBodySetRotation(itsHarrierBody, R);
  dMassSetZero(&mass);
  dMassSetBoxTotal(&mass, itsHarrierWeight,itsHarrierWidth, itsHarrierLength, 0.5);
  //dMassSetCappedCylinderTotal(&mass,itsHarrierWeight,3,itsHarrierWidth,itsHarrierLength/2);
  dMassRotate(&mass, R);
  dBodySetMass(itsHarrierBody,&mass);
  itsHarrierGeom = dCreateBox(space, itsHarrierWidth, itsHarrierLength, 0.5);
  dGeomSetRotation(itsHarrierGeom, R);
  dGeomSetBody(itsHarrierGeom, itsHarrierBody);
}
示例#30
0
void CCylinderGeom::set_position(const Fvector& ref_point)
{

	inherited::set_position(ref_point);
	dVector3 local_position={
	 m_cylinder.m_center.x-ref_point.x,
	 m_cylinder.m_center.y-ref_point.y,
	 m_cylinder.m_center.z-ref_point.z
	};

	dGeomSetPosition(
			geom(),
			local_position[0],
			local_position[1],
			local_position[2]
			);
		dMatrix3 R;
		Fmatrix33 m33;
		m33.j.set(m_cylinder.m_direction);
		Fvector::generate_orthonormal_basis(m33.j,m33.k,m33.i);
		PHDynamicData::FMX33toDMX(m33,R);
		dGeomSetRotation(geom(),R);
}