void dGeomSetRotation (dxGeom *g, const dMatrix3 R)
{
dAASSERT (g && R);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
CHECK_NOT_LOCKED (g->parent_space);
if (g->offset_posr) {
g->recomputePosr();
// move body such that body+offset = rotation
dxPosR new_final_posr;
dxPosR new_body_posr;
memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
memcpy(new_final_posr.R, R, sizeof(dMatrix3));
getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr);
dBodySetRotation(g->body, new_body_posr.R);
dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]);
}
else if (g->body) {
// this will call dGeomMoved (g), so we don't have to
dBodySetRotation (g->body,R);
}
else {
memcpy (g->final_posr->R,R,sizeof(dMatrix3));
dGeomMoved (g);
}
}
/*** 車輪の生成 ***/
void makeWheel()
{
dMass mass;
dReal r = 0.1;
dReal w = 0.024;
dReal m = 0.15;
dReal d = 0.01;
dReal tmp_z = Z;
dReal wx[WHEEL_NUM] = {SIDE[0]/2+w/2+d, - (SIDE[0]/2+w/2+d), 0, 0};
dReal wy[WHEEL_NUM] = {0, 0, SIDE[1]/2+w/2+d, - (SIDE[1]/2+w/2+d)};
dReal wz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z};
dReal jx[WHEEL_NUM] = {SIDE[0]/2, - SIDE[0]/2, 0, 0};
dReal jy[WHEEL_NUM] = {0, 0, SIDE[1]/2, - SIDE[1]/2};
dReal jz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z};
for (int i = 0; i < WHEEL_NUM; i++)
{
wheel[i].v = 0.0;
// make body, geom and set geom to body.
// The position and orientation of geom is abondoned.
wheel[i].body = dBodyCreate(world);
wheel[i].geom = dCreateCylinder(space,r, w);
dGeomSetBody(wheel[i].geom,wheel[i].body);
// set configure of body
dMassSetZero(&mass);
if (i < 2) {
dMassSetCylinderTotal(&mass,m, 1, r, w);
} else {
dMassSetCylinderTotal(&mass,m, 2, r, w);
}
dBodySetMass(wheel[i].body,&mass);
// set position and orientation of body.
dBodySetPosition(wheel[i].body, wx[i], wy[i], wz[i]);
dMatrix3 R;
if (i >= 2) {
dRFromAxisAndAngle(R,1,0,0,M_PI/2.0);
dBodySetRotation(wheel[i].body,R);
} else {
dRFromAxisAndAngle(R,0,1,0,M_PI/2.0);
dBodySetRotation(wheel[i].body,R);
}
// make joint.
wheel[i].joint = dJointCreateHinge(world,0);
dJointAttach(wheel[i].joint,base.body,wheel[i].body);
if (i < 2) {
dJointSetHingeAxis(wheel[i].joint, 1, 0, 0);
} else {
dJointSetHingeAxis(wheel[i].joint, 0, -1, 0);
}
dJointSetHingeAnchor(wheel[i].joint, jx[i], jy[i], jz[i]);
}
}
void place_cards()
{
ncards = getncards(levels);
// destroy removed cards (if any)
int oldcards = cards.size();
for (int i=ncards; i<oldcards; ++i)
delete cards[i];
cards.resize(ncards);
// construct new cards (if any)
for (int i=oldcards; i<ncards; ++i)
cards[i] = new Card;
// for each level
int c = 0;
dMatrix3 right, left, hrot;
dReal angle = 20*M_PI/180.;
dRFromAxisAndAngle(right, 1, 0, 0, -angle);
dRFromAxisAndAngle(left, 1, 0, 0, angle);
dRFromAxisAndAngle(hrot, 1, 0, 0, 91*M_PI/180.);
dReal eps = 0.05;
dReal vstep = cos(angle)*clength + eps;
dReal hstep = sin(angle)*clength + eps;
for (int lvl=0; lvl<levels; ++lvl) {
// there are 3*(levels-lvl)-1 cards in each level, except last
int n = (levels-lvl);
dReal height = (lvl)*vstep + vstep/2;
// inclined cards
for (int i=0; i<2*n; ++i, ++c) {
dBodySetPosition(cards[c]->body,
0,
-n*hstep + hstep*i,
height
);
if (i%2)
dBodySetRotation(cards[c]->body, left);
else
dBodySetRotation(cards[c]->body, right);
}
if (n==1) // top of the house
break;
// horizontal cards
for (int i=0; i<n-1; ++i, ++c) {
dBodySetPosition(cards[c]->body,
0,
-(n-1 - (clength-hstep)/2)*hstep + 2*hstep*i,
height + vstep/2);
dBodySetRotation(cards[c]->body, hrot);
}
}
}
void createInvisibleHead( float* pos )
{
dMatrix3 head_orientation;
dRFromEulerAngles(head_orientation, 0.0, 0.0, 0.0);
//position and orientation
head.Body = dBodyCreate(World);
dBodySetPosition(head.Body, pos[ 0 ], pos[ 1 ], pos[ 2 ]);
dBodySetRotation(head.Body, head_orientation);
dBodySetLinearVel(head.Body, 0, 0, 0);
dBodySetData(head.Body, (void *)0);
//mass
dMass head_mass;
dMassSetBox(&head_mass, 1.0, 1.0, 1.0, 1.0);
dBodySetMass(head.Body, &head_mass);
//geometry
head.Geom = dCreateBox(Space, 1.0, 1.0, 1.0);
dGeomSetBody(head.Geom, head.Body);
//fixed joint
invis_box_joint = dJointCreateFixed(World, jointgroup);
dJointAttach(invis_box_joint, body.Body, head.Body);
dJointSetFixed(invis_box_joint);
}
CubeBasePiece::CubeBasePiece(dWorldID& world,dSpaceID& space, float x, float y, float z)
{
body = dBodyCreate(world);
geom = dCreateBox(space, sides[0], sides[1], sides[2]);
dGeomSetBody(geom, body);
dGeomSetData(geom, this);
dMass mass;
mass.setBox(CUBE_PIECE_DENSITY, sides[0], sides[1], sides[2]);
dBodySetMass(body, &mass);
const dMatrix3 rotationMatrix = {1,0,0,0,
0,1,0,0,
0,0,1,0};
dBodySetRotation(body, rotationMatrix);
dBodySetPosition(body,x,y,z);
for(int i = 0 ; i < 6 ; i++)
attachedPieces[i] = NULL; // initialize attached piece array to all null pointers
color[0] = 1;
color[1] = 1;
color[2] = 1;
}
void set_phys_rotation(dBodyID body, const float *r)
{
dMatrix3 R;
set_rotation(R, r);
dBodySetRotation(body, R);
}
//===========================================================================
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);
}
}
WheelPiece::WheelPiece(dWorldID& world,dSpaceID& space, float x, float y, float z)
{
dBodyCreate(world);
body = dBodyCreate(world);
radius = .35;
thickness = .25;
activationDirection = 0;
geom = dCreateCylinder(space, radius, thickness);
dGeomSetBody(geom, body);
dGeomSetData(geom, this);
dMass mass;
mass.setCylinder(.5, 1, radius, thickness);
dBodySetMass(body, &mass);
const dMatrix3 rotationMatrix = {1,0,0,0,
0,1,0,0,
0,0,1,0
};
dBodySetRotation(body, rotationMatrix);
dBodySetPosition(body,x,y,z);
attachmentOffset = thickness + .25;
color[0] = .5;
color[1] = 1;
color[2] = 1;
}
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 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());
}
}
/*
=================================================================================
createFixedLeg
Use parameters to create leg body/geom and attach to body with fixed joint
=================================================================================
*/
void createFixedLeg(ODEObject &leg,
ODEObject &bodyAttachedTo,
dJointID& joint,
dReal xPos, dReal yPos, dReal zPos,
dReal xRot, dReal yRot, dReal zRot,
dReal radius,
dReal length)
{
dMatrix3 legOrient;
dRFromEulerAngles(legOrient, xRot, yRot, zRot);
//position and orientation
leg.Body = dBodyCreate(World);
dBodySetPosition(leg.Body, xPos, yPos, zPos);
dBodySetRotation(leg.Body, legOrient);
dBodySetLinearVel(leg.Body, 0, 0, 0);
dBodySetData(leg.Body, (void *)0);
//mass
dMass legMass;
dMassSetCapsule(&legMass, 1, 3, radius, length);
dBodySetMass(leg.Body, &legMass);
//geometry
leg.Geom = dCreateCapsule(Space, radius, length);
dGeomSetBody(leg.Geom, leg.Body);
//fixed joint
joint = dJointCreateFixed(World, jointgroup);
dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
dJointSetFixed(joint);
}
void ODERigidObject::SetTransform(const RigidTransform& T)
{
Vector3 comPos = T*obj.com;
dBodySetPosition(bodyID,comPos.x,comPos.y,comPos.z);
dMatrix3 rot;
CopyMatrix(rot,T.R);
dBodySetRotation(bodyID,rot);
}
/* sets the orientation axes of a body */
void body_set_axes (dBodyID b, const t_real *u, const t_real *v, const t_real *t) {
dMatrix3 r;
r [0] = u[0]; r [4] = u[1]; r [8] = u[2];
r [1] = v[0]; r [5] = v[1]; r [9] = v[2];
r [2] = t[0]; r [6] = t[1]; r [10] = t[2];
r [3] = 0.; r [7] = 0.; r [11] = 0.;
dBodySetRotation (b, r);
}
void ODE_Link::setTransform(const hrp::Vector3& pos, const hrp::Matrix33& R){
hrp::Vector3 _pos(R * C + pos);
dBodySetPosition(bodyId, _pos(0), _pos(1), _pos(2));
dMatrix3 _R = {R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), 0};
dBodySetRotation(bodyId, _R);
}
static void simLoop (int pause)
{
// stop after a given number of iterations, as long as we are not in
// interactive mode
if (cmd_graphics && !cmd_interactive &&
(iteration >= max_iterations)) {
dsStop();
return;
}
iteration++;
if (!pause) {
// do stuff for this test and check to see if the joint is behaving well
dReal error = doStuffAndGetError (test_num);
if (error > max_error) max_error = error;
if (cmd_interactive && error < dInfinity) {
printf ("scaled error = %.4e\n",error);
}
// take a step
dWorldStep (world,STEPSIZE);
// occasionally re-orient the first body to create a deliberate error.
if (cmd_occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
}
if (cmd_graphics) {
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
if (body[1]) {
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
}
}
void TSRODERigidBody::SetBodyTransform( const TSRMatrix4& _bodyTransform )
{
dMatrix4 R;
dVector3 P;
Matrix4ToODE( _bodyTransform, R, P );
dBodySetPosition( m_BodyID, P[ 0 ], P[ 1 ], P[ 2 ] );
dBodySetRotation( m_BodyID, R );
}
static void simLoop (int pause)
{
const dReal kd = -0.3; // angular damping constant
const dReal ks = 0.5; // spring constant
if (!pause) {
// add an oscillating torque to body 0, and also damp its rotational motion
static dReal a=0;
const dReal *w = dBodyGetAngularVel (body[0]);
dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
a += 0.01;
// add a spring force to keep the bodies together, otherwise they will
// fly apart along the slider axis.
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),
ks*(p2[2]-p1[2]));
dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),
ks*(p1[2]-p2[2]));
// occasionally re-orient one of the bodies to create a deliberate error.
if (occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
dWorldStep (world,0.05);
}
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
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);
}
void Wheel::disposePhysics(Utils::Xml &x) {
dMatrix3 R;
dRFromAxisAndAngle(R, 0.0, 1.0, 0.0,
Ogre::Degree(x.mustOReal("rotation.y")).valueRadians());
dBodySetRotation(ph.body, R);
Utils::Xml w("../xml/car.xml", "car");
dGeomSetPosition(ph.geom,
w.mustOReal("global-position.x") + x.mustOReal("position.x"),
w.mustOReal("global-position.y") + x.mustOReal("position.y"),
w.mustOReal("global-position.z") + x.mustOReal("position.z")
);
}
void Car::createBody(const dReal * pos)
{
// create body
std::cout << "debug: in createBody\n";
body_obj =
new BoxObject(world, space, car_design->getBodySides(),
body_mass);
dBodySetPosition(body_obj->body[0], pos[XX], pos[YY], pos[ZZ]);
dBodySetRotation(body_obj->body[0],
car_design->getCenterRotation());
//dBodySetRotation(body_obj->body[0], R);
}
void dGeomSetRotation (dxGeom *g, const dMatrix3 R)
{
dAASSERT (g && R);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
CHECK_NOT_LOCKED (g->parent_space);
if (g->body) {
// this will call dGeomMoved (g), so we don't have to
dBodySetRotation (g->body,R);
}
else {
memcpy (g->R,R,sizeof(dMatrix3));
dGeomMoved (g);
}
}
void CappedCylinder::createBody()
{
if(body == 0)
{
body = dBodyCreate(*simulation->getPhysicalWorld());
dMass m;
dMassSetCapsule(&m, 1, 3, dReal(height), dReal(height));
dMassAdjust(&m, dReal(mass));
dBodySetMass(body, &m);
dBodySetPosition(body, dReal(position.v[0]), dReal(position.v[1]), dReal(position.v[2]));
dMatrix3 rotationMatrix;
ODETools::convertMatrix(rotation, rotationMatrix);
dBodySetRotation(body, rotationMatrix);
}
}
void Machine::reset(void)
{
dMatrix3 R;
if(this==&machine[0])
{
dRFromAxisAndAngle(R, 0, 0, 1, -M_PI/2.0);
dBodySetPosition(body[0], -court.x/2, 0, 26.5);
dBodySetRotation(body[0], R);
dBodySetPosition(body[1], -court.x/2+17, 0, 26.5);
dBodySetRotation(body[1], R);
int i;
for(i=0; i<3; i++)
dBodySetRotation(wheel[i], R);
dBodySetPosition(wheel[0], 12-court.x/2, 0, 26);
dBodySetPosition(wheel[1], -7-court.x/2, 6, 26);
dBodySetPosition(wheel[2], -7-court.x/2, -6, 26);
}
else
{
dRFromAxisAndAngle(R, 0, 0, 1, M_PI/2.0);
dBodySetPosition(body[0], court.x/2, 0, 26.5);
dBodySetRotation(body[0], R);
dBodySetPosition(body[1], court.x/2-17, 0, 26.5);
dBodySetRotation(body[1], R);
int i;
for(i=0; i<3; i++)
dBodySetRotation(wheel[i], R);
dBodySetPosition(wheel[0], -12+court.x/2, 0, 26);
dBodySetPosition(wheel[1], 7+court.x/2, -6, 26);
dBodySetPosition(wheel[2], 7+court.x/2, 6, 26);
}
steer=0;
pushtime=0;
energy=4;
adjuststeer();
dBodySetLinearVel(body[0], 0, 0, 0);
dBodySetLinearVel(body[1], 0, 0, 0);
dBodySetAngularVel(body[0], 0, 0, 0);
dBodySetAngularVel(body[1], 0, 0, 0);
int i;
for(i=0; i<3; i++)
{
dBodySetLinearVel(wheel[i], 0, 0, 0);
dBodySetAngularVel(wheel[i], 0, 0, 0);
}
upsidedowntime=0;
shieldcount=0;
meshatekcount=0;
powerupammo=0;
powerupcount=0;
powerupcharge=0;
}
/*
=================================================================================
createUniversalLeg
Use parameters to create leg body/geom and attach to body with universal joint
**Warning**
mass is not set
=================================================================================
*/
void createUniversalLeg(ODEObject &leg,
ODEObject &bodyAttachedTo,
dJointID& joint,
dReal xPos, dReal yPos, dReal zPos,
dReal xRot, dReal yRot, dReal zRot,
dReal radius, dReal length,
dReal maxAngle, dReal minAngle,
dReal anchorXPos, dReal anchorYPos, dReal anchorZPos)
{
dMatrix3 legOrient;
dRFromEulerAngles(legOrient, xRot, yRot, zRot);
//position and orientation
leg.Body = dBodyCreate(World);
dBodySetPosition(leg.Body, xPos, yPos, zPos);
dBodySetRotation(leg.Body, legOrient);
dBodySetLinearVel(leg.Body, 0, 0, 0);
dBodySetData(leg.Body, (void *)0);
//mass
dMass legMass;
dMassSetCapsule(&legMass, 1, 3, radius, length);
//dBodySetMass(leg.Body, &legMass);
//geometry
leg.Geom = dCreateCapsule(Space, radius, length);
dGeomSetBody(leg.Geom, leg.Body);
//universal joint
joint = dJointCreateUniversal(World, jointgroup);
//attach and anchor
dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
dJointSetUniversalAnchor(joint, anchorXPos, anchorYPos, anchorZPos);
//axes
dJointSetUniversalAxis1(joint, 0, 0, 1);
dJointSetUniversalAxis2(joint, 0, 1, 0);
//Max and min angles
dJointSetUniversalParam(joint, dParamHiStop, maxAngle);
dJointSetUniversalParam(joint, dParamLoStop, minAngle);
dJointSetUniversalParam(joint, dParamHiStop2, maxAngle);
dJointSetUniversalParam(joint, dParamLoStop2, minAngle);
}
void WheelPiece::attachToBase(dBodyID otherBody, dWorldID world, dJointGroupID jointGroup, dReal x, dReal y, dReal z, const dMatrix3 rotationMatrix)
{
// set this piece position and rotation
dBodySetPosition(body, x, y, z);
dBodySetRotation(body, rotationMatrix);
// create and connect
connectingJoint = dJointCreateHinge2(world,0);
dJointAttach (connectingJoint,otherBody,body);
// set anchor, axes, and lo and hi stops
const dReal *a = dBodyGetPosition (body);
dJointSetHinge2Anchor (connectingJoint,a[0],a[1],a[2]);
dJointSetHinge2Axis1 (connectingJoint,rotationMatrix[1],rotationMatrix[5],rotationMatrix[9]);
dJointSetHinge2Axis2 (connectingJoint,rotationMatrix[2],rotationMatrix[6],rotationMatrix[10]);
dJointSetHinge2Param (connectingJoint,dParamLoStop,0);
dJointSetHinge2Param (connectingJoint,dParamHiStop,0);
}
void resetBall(dBodyID b, unsigned idx)
{
dBodySetPosition(b,
0.5*track_len*cos(track_incl) // Z
- 0.5*track_height*sin(track_incl)
- ball_radius, // X
balls_sep*idx, // Y
track_elevation + ball_radius// Z
+ 0.5*track_len*sin(track_incl)
+ 0.5*track_height*cos(track_incl));
dMatrix3 r = {1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0};
dBodySetRotation(b, r);
dBodySetLinearVel(b, 0, 0, 0);
dBodySetAngularVel(b, 0, 0, 0);
}
box::box(dWorldID& world, dSpaceID& space, float x, float y, float z)
{
dBodyCreate(world);
body = dBodyCreate(world);
geom = dCreateBox(space, sides[0], sides[1], sides[2]);
dGeomSetBody(geom, body);
dGeomSetData(geom, this);
dMass mass;
mass.setBox(1, sides[0], sides[1], sides[2]);
dBodySetMass(body, &mass);
const dMatrix3 rotationMatrix = {1,0,0,0,
0,1,0,0,
0,0,1,0};
dBodySetRotation(body, rotationMatrix);
dBodySetPosition(body,x,y,z);
}
void SkidSteeringVehicle::create() {
this->vehicleBody = dBodyCreate(this->environment->world);
this->vehicleGeom = dCreateBox(this->environment->space, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
dMassSetBox(&this->vehicleMass, this->density, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
dBodySetMass(this->vehicleBody, &this->vehicleMass);
dBodySetPosition(this->vehicleBody, this->xOffset, this->yOffset, this->zOffset);
dGeomSetBody(this->vehicleGeom, this->vehicleBody);
dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->wheelRadius);
dReal w = this->vehicleBodyWidth + this->wheelWidth + 2 * this->trackVehicleSpace;
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
this->wheelGeom[fr][lr] = dCreateCylinder(this->environment->space, this->wheelRadius, this->wheelWidth);
this->environment->setGeomName(this->wheelGeom[fr][lr], this->name + "." + (!fr ? "front" : "rear") + (!lr ? "Left" : "Right") + "Wheel");
dGeomSetCategoryBits(this->wheelGeom[fr][lr], Category::TRACK_GROUSER);
dGeomSetCollideBits(this->wheelGeom[fr][lr], Category::TERRAIN);
dMassSetCylinder(&this->wheelMass[fr][lr], this->density, 3, this->wheelRadius, this->wheelWidth);
this->wheelBody[fr][lr] = dBodyCreate(this->environment->world);
dBodySetMass(this->wheelBody[fr][lr], &this->wheelMass[fr][lr]);
dGeomSetBody(this->wheelGeom[fr][lr], this->wheelBody[fr][lr]);
dBodySetPosition(this->wheelBody[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + w * (lr - 0.5), this->zOffset);
dMatrix3 wheelR;
dRFromZAxis(wheelR, 0, 2 * lr - 1, 0);
dBodySetRotation(this->wheelBody[fr][lr], wheelR);
this->wheelJoint[fr][lr] = dJointCreateHinge(this->environment->world, 0);
dJointAttach(this->wheelJoint[fr][lr], this->vehicleBody, this->wheelBody[fr][lr]);
dJointSetHingeAnchor(this->wheelJoint[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + this->vehicleBodyWidth * (lr - 0.5), this->zOffset);
dJointSetHingeAxis(this->wheelJoint[fr][lr], 0, 1, 0);
dJointSetHingeParam(this->wheelJoint[fr][lr], dParamFMax, 5.0);
}
}
this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
dRigidBodyArrayAdd(this->bodyArray, this->wheelBody[fr][lr]);
}
}
}
void dRigidBodyArraySetRotation(dRigidBodyArrayID bodyArray, const dReal *Rs) {
dBodyID center = bodyArray->center;
const dReal *p0 = dBodyGetPosition(center);
const dReal *R0 = dBodyGetRotation(center);
dMatrix3 RsR0t;
dMULTIPLY2_333(RsR0t, Rs, R0);
for(size_t i = 0; i < dRigidBodyArraySize(bodyArray); i++) {
dBodyID body = dRigidBodyArrayGet(bodyArray, i);
const dReal *pi = dBodyGetPosition(body);
const dReal *Ri = dBodyGetRotation(body);
dMatrix3 R1;
dMULTIPLY0_333(R1, RsR0t, Ri);
dVector3 p1, pi_p0, R0RsR0t__pi_p0;
dOP(pi_p0, -, pi, p0);
dMULTIPLY0_331(R0RsR0t__pi_p0, RsR0t, pi_p0);
dOP(p1, +, R0RsR0t__pi_p0, p0);
dBodySetPosition(body, p1[0], p1[1], p1[2]);
dBodySetRotation(body, R1);
}
}
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);
}
