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.stop = 0;
fn.command = 0;
fn.path_to_textures = "../../drawstuff/textures";
dInitODE ();
// create world
world = dWorldCreate ();
space = dHashSpaceCreate (0);
dWorldSetGravity (world,0,0,0); //Original Gravity = -0.2
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
contactgroup = dJointGroupCreate (0);
// create object
sphere0 = dBodyCreate (world);
sphere0_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere0,&m);
dGeomSetBody (sphere0_geom,sphere0);
sphere1 = dBodyCreate (world);
sphere1_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere1,&m);
dGeomSetBody (sphere1_geom,sphere1);
sphere2 = dBodyCreate (world);
sphere2_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere2,&m);
dGeomSetBody (sphere2_geom,sphere2);
// set initial position
dBodySetPosition (sphere0,0,0,4);
dBodySetPosition (sphere1,5,0,4);
dBodySetPosition (sphere2,-2,0,4);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
// clean up
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
// Create a ball and a pole
void createBallandPole() {
dMass m1;
dReal x0 = 0.0, y0 = 0.0, z0 = 2.5;
// ball
ball.radius = 0.2;
ball.mass = 1.0;
ball.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetSphereTotal(&m1, ball.mass, ball.radius);
dBodySetMass(ball.body, &m1);
dBodySetPosition(ball.body, x0, y0, z0);
ball.geom = dCreateSphere(space, ball.radius);
dGeomSetBody(ball.geom, ball.body);
// pole
pole.radius = 0.025;
pole.length = 1.0;
pole.mass = 1.0;
pole.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetCapsule(&m1, pole.mass, 3, pole.radius, pole.length);
dBodySetMass(pole.body, &m1);
dBodySetPosition(pole.body, x0, y0, z0 - 0.5 * pole.length);
pole.geom = dCreateCCylinder(space, pole.radius, pole.length);
dGeomSetBody(pole.geom, pole.body);
// hinge joint
joint = dJointCreateHinge(world, 0);
dJointAttach(joint, ball.body, pole.body);
dJointSetHingeAnchor(joint, x0, y0, z0 - ball.radius);
dJointSetHingeAxis(joint, 1, 0, 0);
}
void TSRODERigidBody::AddSphereGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, float _fRadius, 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 = dCreateSphere( 0, _fRadius );
dMassSetSphere( &currMass, _fDensity, _fRadius );
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 );
}
void CPHActivationShape::Create(const Fvector start_pos,const Fvector start_size,IPhysicsShellHolder* ref_obj,EType _type/*=etBox*/,u16 flags)
{
VERIFY(ref_obj);
R_ASSERT(_valid( start_pos ) );
R_ASSERT( _valid( start_size ) );
m_body = dBodyCreate (0) ;
dMass m;
dMassSetSphere(&m,1.f,100000.f);
dMassAdjust(&m,1.f);
dBodySetMass(m_body,&m);
switch(_type)
{
case etBox:
m_geom = dCreateBox (0,start_size.x,start_size.y,start_size.z) ;
break;
case etSphere:
m_geom = dCreateSphere (0,start_size.x);
break;
};
dGeomCreateUserData (m_geom) ;
dGeomUserDataSetObjectContactCallback(m_geom,ActivateTestDepthCallback) ;
dGeomUserDataSetPhysicsRefObject(m_geom,ref_obj) ;
dGeomSetBody (m_geom,m_body) ;
dBodySetPosition (m_body,start_pos.x,start_pos.y,start_pos.z) ;
Island() .AddBody (m_body) ;
dBodyEnable (m_body) ;
m_safe_state .create(m_body) ;
spatial_register () ;
m_flags.set(flags,TRUE);
}
dGeomID SphereGeometry::createGeometry(dSpaceID space)
{
Geometry::createGeometry(space);
approxRadius = radius;
approxRadiusSqr = approxRadius * approxRadius;
return dCreateSphere(space, radius);
}
void sphere::create_physical_body(
double x,
double y,
double z,
double radius,
double mass,
manager& mgr)
{
this->radius = radius;
world_id = mgr.ode_world();
space_id = mgr.ode_space();
//create and position the geom to represent the physical shape of the rigid body
geom_id = dCreateSphere(mgr.ode_space(),radius);
object::set_geom_data(geom_id);
dGeomSetPosition (geom_id, x, y, z);
if(mass > 0)
{
object::create_rigid_body(x, y, z, mgr);
dGeomSetBody (geom_id, body_id);
set_mass(mass);
}
}
/*
* Create microscopic sphere as a dummy parts
* Error will arise if the radius is too small
* Do not set mass
* Do not detect collision between other parts
*/
void SBlindParts::set(dWorldID w, dSpaceID space)
{
double rad = 1.0;
dGeomID geom = dCreateSphere(0, rad);
m_odeobj = ODEObjectContainer::getInstance()->createODEObj
(
w,
geom,
0.9,
0.01,
0.5,
0.5,
0.8,
0.001,
0.0
);
dBodyID body = m_odeobj->body();
/*
dMass m;
dMassSetZero(&m);
dMassSetSphere(&m, 0.0001, rad);
dBodySetMass(body, &m);
*/
m_rot.setQuaternion(1.0, 0.0, 0.0, 0.0);
//dSpaceAdd(space, geom);
dBodySetData(body, this);
}
OscSphereODE::OscSphereODE(dWorldID odeWorld, dSpaceID odeSpace, const char *name, OscBase *parent)
: OscSphere(NULL, name, parent)
{
dGeomID odeGeom = dCreateSphere(odeSpace, m_radius.m_value);
m_pSpecial = new ODEObject(this, odeGeom, odeWorld, odeSpace);
m_density.set(m_density.m_value);
}
SphereGeom::SphereGeom(Body* in_pBody, Space* in_pSpace, double radius)
: Geom(in_pSpace)
{
m_pBody = in_pBody;
dMassSetSphere(&m_mass,1,radius);
dMassAdjust(&m_mass,1);
m_id = dCreateSphere(m_pSpace->id(),radius);
finishGeom();
}
void TurbulentDispersionScene_4Types::Generate()
{
dReal radius0, pos_y;
//dReal pos_x=0.008;
dReal pos_x=1.22916;
dReal pos_z=0.25;
dReal vel_x=6.55;
for(int particle_counter=0;particle_counter<4000;particle_counter++)
{
if(particle_counter>=1000&&particle_counter<2000)
{
DENSITY=2500; //density of glass particles
radius0=43.5;
pos_y=0.25;
}
else if(particle_counter>=2000&&particle_counter<3000)
{
DENSITY=1000; //density of corn pollen particles
radius0=43.5;
pos_y=0.75;
}
else if(particle_counter>=3000&&particle_counter<4000)
{
DENSITY=260; //density of hollow glass particles
radius0=23.25;
pos_y=1.25;
}
else
{
DENSITY=8900; //density of copper particles
radius0=23.25;
pos_y=1.75;
}
radius0=radius0*0.000001;
dBodyID body=dBodyCreate(domain->getWorld());
dGeomID geom=dCreateSphere(domain->getSpace(),radius0);
//Particle *p=new Particle(body,geom);
ODE_Particle *p=new ODE_Particle(body,geom);
p->setMass(DENSITY,radius0);
p->setCategoryBits(catBits[PARTICLES]);
p->setCollideBits(catBits[WALLS]&(~catBits[GENERATORS]));
p->setPosition(pos_x,pos_y,pos_z);
p->setLinearVel(vel_x,0,0);
p->setTTurbInt(0);
p->setTTurb(0);
domain->AddDynamicBody(p);
}
}
void ode::newLine(float xp, float yp)
{
b = dBodyCreate (world);
dBodySetPosition (b,xp,yp,2);
//dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
lineStrips[lines] = dCreateSphere (space,RADIUS);
dGeomSetBody (lineStrips[lines++],b);
}
PhySphere::PhySphere(eType type)
: PhyEntity(type)
{
mGeomId = dCreateSphere(g_OdeSpace, 1);
if (mBodyId != NULL)
dGeomSetBody(mOdeGeom, mOdeBody);
dGeomSetData(mOdeGeom, this);
}
//===========================================================================
void cODEGenericBody::createDynamicSphere(const double a_radius,
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 sphere
m_ode_geom = dCreateSphere(m_ODEWorld->m_ode_space, a_radius);
// 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)
{
dMassSetSphere(&m_ode_mass, 1.0, a_radius);
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_SPHERE;
// store dynamic model parameters
m_paramDynColModel0 = a_radius;
m_paramDynColModel1 = 0.0;
m_paramDynColModel2 = 0.0;
m_posOffsetDynColModel = a_offsetPos;
m_rotOffsetDynColModel = a_offsetRot;
}
void ode::newBall(int xp, int yp)
{
dReal z=1,y=1,x=1;
b = dBodyCreate (world);
dBodySetPosition (b,xp,yp,2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
int main (int argc, char **argv)
{
int i;
dReal k;
dMass m;
/* setup pointers to drawstuff callback functions */
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = "../../drawstuff/textures";
if(argc==2)
{
fn.path_to_textures = argv[1];
}
/* create world */
dInitODE();
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (1000000);
dWorldSetGravity (world,0,0,-0.5);
dCreatePlane (space,0,0,1,0);
for (i=0; i<NUM; i++) {
body[i] = dBodyCreate (world);
k = i*SIDE;
dBodySetPosition (body[i],k,k,k+0.4);
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
dBodySetMass (body[i],&m);
sphere[i] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[i],body[i]);
}
for (i=0; i<(NUM-1); i++) {
joint[i] = dJointCreateBall (world,0);
dJointAttach (joint[i],body[i],body[i+1]);
k = (i+0.5)*SIDE;
dJointSetBallAnchor (joint[i],k,k,k+0.4);
}
/* run simulation */
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void cPhysicsObject::CreateSphere(cWorld* pWorld, const math::cVec3& pos, const math::cVec3& rot)
{
geom = dCreateSphere(bDynamic ? pWorld->GetSpaceDynamic() : pWorld->GetSpaceStatic(), fRadius);
InitCommon(pWorld, pos, rot);
if (bBody) {
dMass mass;
dMassSetSphereTotal(&mass, fMassKg, 2.0f * fRadius);
dBodySetMass(body, &mass);
}
}
void Wheel::createPhysics(Utils::Xml &x, dSpaceID space) {
if (x.mustString("nature") == "sphere") {
ph.geom = dCreateSphere(space, x.mustOReal("radius"));
dMassSetSphereTotal(&ph.mass, x.mustOReal("mass"), x.mustOReal("radius"));
}
else {
ph.geom = dCreateCylinder(space, x.mustOReal("radius"), x.mustOReal("width"));
dMassSetCylinderTotal(&ph.mass, x.mustOReal("mass"), 1, x.mustOReal("radius"), x.mustOReal("width"));
}
ph.body = World::getSingletonPtr()->add(ph.geom, &ph.mass);
}
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);
}
WheelItem::WheelItem(dWorldID world,dSpaceID space,dQuaternion q,dReal radius,dReal mass)
{
body = dBodyCreate(world);
geom = dCreateSphere(space,radius);
dBodySetQuaternion(body,q);
dMass m;
dMassSetSphere(&m,1,radius);
dMassAdjust(&m,mass);
dBodySetMass(body,&m);
dGeomSetBody(geom,body);
}
//since this is just the base level construction, we'll just make a sphere (WHOO, EXCITING!!!!)
bool Construction::Construct( char* descriptionFile, DynamicsSolver* solver, Screen3D& Screen,MeshManager& MM, Position& Location, ICollisionHandler* ch )
{
//deconstruct any old stuff
Deconstruct();
//save the solver pointer
mySolver = solver;
CollisionHandler = ch;
//create the body list
ObjectList = new DynamicsObject[1];
this->nObjects = 1;
//Create the geom group
GeomGroup = dSimpleSpaceCreate (solver->GetSpaceID(false)); //dCreateGeomGroup (solver->GetSpaceID(false));
//Create the actual body ( a sphere! )
ObjectList[0].CreateBody( solver );
dBodySetPosition ( ObjectList[0].Body, Location.x, Location.y, Location.z);
dMassSetSphere ( &ObjectList[0].Mass, 1.0, 5.0 );
dMassAdjust (&ObjectList[0].Mass, 1.0 );
dBodySetMass( ObjectList[0].Body, &ObjectList[0].Mass);
ObjectList[0].Geom = dCreateSphere (0,5.0);
dGeomSetData( ObjectList[0].Geom, &ObjectList[0].SurfaceDesc );
dGeomSetBody (ObjectList[0].Geom,ObjectList[0].Body);
dSpaceAdd (GeomGroup,ObjectList[0].Geom);
ObjectList[0].HasGeom = true;
//set owner
for(int i=0; i<nObjects; i++)
{
ObjectList[i].SurfaceDesc.Owner = &ObjectList[i];
ObjectList[i].SurfaceDesc.ParentConstruction = this;
ObjectList[i].Owner = this;
//ObjectList[i].HasGeom = true;
}
LinearDisableEpsilon = .1;
AngularDisableEpsilon = .01f;
//create the mesh for drawing
D3DXCreateSphere( Screen.D3DDevice, 5.5f, 10, 10, &DrawMesh, NULL );
return true;
}
dGeomID set_phys_geom_type(dGeomID geom, dBodyID body,
int i, int t, const float *v)
{
/* Destroy the old geom and its data. */
if (geom)
{
free(dGeomGetData(geom));
dGeomDestroy(geom);
geom = 0;
}
/* Create a new geom of the required type. */
switch (t)
{
case dSphereClass:
geom = dCreateSphere(space, v[0]);
break;
case dCapsuleClass:
geom = dCreateCapsule(space, v[0], v[1]);
break;
case dBoxClass:
geom = dCreateBox(space, v[0], v[1], v[2]);
break;
case dPlaneClass:
geom = dCreatePlane(space, v[0], v[1], v[2], v[3]);
break;
case dRayClass:
geom = dCreateRay(space, (dReal) sqrt(v[3] * v[3] +
v[4] * v[4] +
v[5] * v[5]));
dGeomRaySet(geom, v[0], v[1], v[2], v[3], v[4], v[5]);
break;
}
/* Assign geom data and attach it to the body. */
if (geom)
{
dGeomSetData(geom, create_data(i));
dGeomSetBody(geom, body);
}
return geom;
}
/*** ボールの生成 ***/
static void makeBall()
{
dMass mass;
ball.m = 0.45;
ball.r = 0.11;
ball.x = 1.0;
ball.y = 0.0;
ball.z = 0.14 + offset_z;
ball.body = dBodyCreate(world);
dMassSetZero(&mass);
dMassSetSphereTotal(&mass,ball.m,ball.r);
dBodySetMass(ball.body,&mass);
ball.geom = dCreateSphere(space,ball.r);
dGeomSetBody(ball.geom,ball.body);
dBodySetPosition(ball.body,ball.x, ball.y, ball.z);
}
MarkerData::MarkerData(dWorldID world,dSpaceID space,QObject *parent) :
QObject(parent)
{
this->world=world;
this->space=space;
data.loadFile("../QtVR/data/data.c3d");
std::cout << "[markerdata] Marker file loaded...";
marker_count = data.data.pointsPerFrame;
std::cout << " ...this file contains " << marker_count << " markers" << std::endl;
old_c_frame = c_frame = new C3dFloatFrame(marker_count);
old_n_frame = n_frame = new C3dFloatFrame(marker_count);
geom = new dGeomID[marker_count];
body = new dBodyID[marker_count];
joint = new dJointID[marker_count];
try_link = new bool[marker_count];
//space = dSimpleSpaceCreate(0);
joint_group = dJointGroupCreate(0);
for (int ii=0;ii<marker_count;++ii) {
body[ii] = dBodyCreate(world);
dBodySetKinematic(body[ii]);
geom[ii] = dCreateSphere(space,.01);
dGeomSetBody(geom[ii],body[ii]);
try_link[ii]=false;
joint[ii] = dJointCreateBall(world,joint_group);
}
frames_per_step=2;
time_per_step = .015;
current_frame = 0;
paused=true;
single_step=false;
use_virtual_markers=false;
grand = new GaussRand;
}
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();
}
void Sphere::init(const OdeHandle& odeHandle, double mass, const OsgHandle& osgHandle,
char mode) {
assert(mode & Body || mode & Geom);
if (!substanceManuallySet)
substance = odeHandle.substance;
this->mode=mode;
QMP_CRITICAL(2);
if (mode & Body){
body = dBodyCreate (odeHandle.world);
setMass(mass, mode & Density);
}
if (mode & Geom){
geom = dCreateSphere ( odeHandle.space , osgsphere->getRadius());
attachGeomAndSetColliderFlags();
}
if (mode & Draw){
osgsphere->init(osgHandle);
}
QMP_END_CRITICAL(2);
}
void dmCreateSphere(dmObject *obj, double p[2], double R[12], double m, double r, double color[3])
{
obj->body = dBodyCreate(world); // ボールの生成
obj->m = m;
obj->r = r;
obj->R = R;
obj->p = p;
obj->color = color;
dMass mass; // 構造体massの宣言
dMassSetZero(&mass); // 構造体massの初期化
dMassSetSphereTotal(&mass,obj->m,obj->r); // 構造体massに質量を設定
dBodySetMass(obj->body,&mass); // ボールにmassを設定
dBodySetPosition(obj->body, obj->p[0], obj->p[1], obj->p[2]); // ボールの位置(x,y,z)を設定
dBodySetRotation(obj->body, obj->R);
obj->geom = dCreateSphere(space,obj->r); // 球ジオメトリの生成
dGeomSetBody(obj->geom, obj->body); // ボディとジオメトリの関連付け
}
static GObject * soy_fields_shockwave_constructor (GType type, guint n_construct_properties, GObjectConstructParam * construct_properties) {
GObject * obj;
GObjectClass * parent_class;
soyfieldsShockwave * self;
struct dxSpace* _tmp0_;
soyscenesScene* _tmp1_;
struct dxSpace* _tmp2_;
struct dxGeom* _tmp3_;
parent_class = G_OBJECT_CLASS (soy_fields_shockwave_parent_class);
obj = parent_class->constructor (type, n_construct_properties, construct_properties);
self = G_TYPE_CHECK_INSTANCE_CAST (obj, SOY_FIELDS_TYPE_SHOCKWAVE, soyfieldsShockwave);
_tmp0_ = dHashSpaceCreate (NULL);
_dSpaceDestroy0 (self->priv->_space);
self->priv->_space = _tmp0_;
_tmp1_ = ((soybodiesBody*) self)->scene;
_tmp2_ = _tmp1_->space;
_tmp3_ = dCreateSphere (_tmp2_, (dReal) 0.0f);
_dGeomDestroy0 (((soybodiesBody*) self)->geom);
((soybodiesBody*) self)->geom = _tmp3_;
return obj;
}
//you should create your own world
void CreateWorld1(Simulator * sim)
{
const double zdim = 2.0;
const double xdim = 20;
const double ydim = 20;
AddBoundaries(sim, xdim, ydim, zdim, 0.2);
dGeomID geom;
dGeomID geom_gara1;
dGeomID geom_gara2;
dQuaternion q; //for rotations
// create a parking garage - 1 st wall
geom_gara1 = dCreateBox(sim->GetSpaceID(), 4.0, 0.2, zdim);
dGeomSetPosition(geom_gara1, -13 + 0.5*(30-xdim), -5, 0.5 * zdim);
dQFromAxisAndAngle(q, 0, 0, 1, M_PI * 1);
dGeomSetQuaternion(geom_gara1, q);
sim->AddGeometryAsObstacle(geom_gara1);
// creat a pakring garage - 2nd wall
geom_gara2 = dCreateBox(sim->GetSpaceID(), 4.0, 0.2, zdim);
dGeomSetPosition(geom_gara2, -13+0.5*(30-xdim), -7.5, 0.5 * zdim);
dQFromAxisAndAngle(q, 0, 0, 1, M_PI * 1);
dGeomSetQuaternion(geom_gara2, q);
sim->AddGeometryAsObstacle(geom_gara2);
geom = dCreateBox(sim->GetSpaceID(), 9.0, 0.2, zdim);
dGeomSetPosition(geom, 6, -4, 0.5 * zdim);
dQFromAxisAndAngle(q, 0, 0, 1, -M_PI * 0.25);
dGeomSetQuaternion(geom, q);
sim->AddGeometryAsObstacle(geom);
geom = dCreateSphere(sim->GetSpaceID(), 0.5 * zdim);
dGeomSetPosition(geom, 0, -4, 0.5 * zdim);
sim->AddGeometryAsObstacle(geom);
}
CRigidCapsule::CRigidCapsule(S32 parent, CSceneObject *so, const CVector3 &d ) : CRigidBody(parent, so) {
if(parent == 0)parent = (S32)space;
#ifdef SIM
mGeomID = dCreateGeomTransform ((dSpaceID)parent);
dGeomTransformSetCleanup (mGeomID, 1);
F32 radius = sqrt(d.x * d.x + d.z * d.z) / 2.0;
//F32 radius = d.x / 2;
dGeomID mGeomSphere = dCreateSphere(0, TO_PHYSICS(radius));
dGeomTransformSetGeom (mGeomID, mGeomSphere);
dGeomSetPosition (mGeomSphere, 0, TO_PHYSICS(-d.y/2 + radius), 0);
mGeomID2 = dCreateGeomTransform ((dSpaceID)parent);
dGeomTransformSetCleanup (mGeomID2, 1);
dGeomID mGeomBox = dCreateBox(0, TO_PHYSICS(d.x), TO_PHYSICS(d.y - radius), TO_PHYSICS(d.z));
dGeomTransformSetGeom (mGeomID2, mGeomBox);
dGeomSetPosition (mGeomBox, 0, TO_PHYSICS(radius / 2), 0);
#else
F32 length = d.y - d.x * 2;
mGeomID = dCreateCCylinder((dSpaceID)parent, TO_PHYSICS(d.x), TO_PHYSICS(length));
#endif
mBodyID = dBodyCreate(world);
dGeomSetBody(mGeomID, mBodyID);
#ifdef SIM
dGeomSetBody(mGeomID2, mBodyID);
#endif
dGeomSetData(mGeomID, static_cast<void *>(this));
#ifdef SIM
dGeomSetData(mGeomID2, static_cast<void *>(this));
#endif
mDimentions = d;
mDimentions.y = length;
setDensity(0.0001);
dBodySetAutoDisableFlag(mBodyID, 0);
mGroundBox.setBounds(CVector3(-d.x, -d.y / 2 - 5.0, -d.z), CVector3(d.x, -d.y / 2 + 20.0, d.z));
}
void CBall::Init()
{
static float dif_orange[4] = {0.75,0.4,0.2,1.0};
static float amb_orange[4] = {0.5,0.2,0.1,1.0};
static float spec[4] = {1.0,1.0,1.0,1.0};
glPushMatrix();
glLoadIdentity();
glTranslatef(0.000, 0.000, 1.100);
glGetDoublev(GL_MODELVIEW_MATRIX, position);
glPopMatrix();
material.SetDiffuse(dif_orange);
material.SetAmbient(amb_orange);
material.SetSpecular(spec);
material.SetShininess(300);
radius = 0.040;
//body設定
bBall = dBodyCreate(world);
dBodySetAutoDisableFlag(bBall,0);
dBodySetPosition(bBall, position[12+0], position[12+1], position[12+2]);
dMass mass;
dMassSetSphereTotal(&(mass),0.010,0.040);
dBodySetMass (bBall ,&(mass));
//geom設定
transBall = dCreateGeomTransform(space);
gBall = dCreateSphere (0,0.040);
dGeomSetPosition(gBall, 0 ,0, 0.002);//重心が3mmずれてる
dGeomTransformSetGeom(transBall, gBall);
dGeomSetBody(transBall, bBall);
}