1 /***************************************************************************
2 * Copyright (C) 2004-2005 by *
3 * Paolo Sacconier <axa1981@tin.it> *
4 * Francesco Tamagni <minchiahead@hacari.org> *
5 * *
6 * This program is free software; you can redistribute it and/or modify *
7 * it under the terms of the GNU General Public License as published by *
8 * the Free Software Foundation; either version 2 of the License, or *
9 * (at your option) any later version. *
10 * *
11 * This program is distributed in the hope that it will be useful, *
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
14 * GNU General Public License for more details. *
15 * *
16 * You should have received a copy of the GNU General Public License *
17 * along with this program; if not, write to the *
18 * Free Software Foundation, Inc., *
19 * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
20 ***************************************************************************/
21 #include "omnicar.h"
22
23 namespace gillo {
24 sgVec3 Omnicar::dirDim = { 0.04, 0.35, 0.04 };
25 // sgVec3 Omnicar::carDim = { 0.04, 0.12, 0.02 };
26 sgVec3 Omnicar::carDim = { 0.05, 0.12, 0.05 };
Omnicar(Context & c,SimpleBall & b,float pi,int plid,int nw)27 Omnicar::Omnicar(Context& c, SimpleBall& b, float pi, int plid, int nw)
28 : Entity(c), ball(b), speed(0), steering(0), backwards(false), jumping(false), steerAngle(0), ballJoint(0), jumpForce(Omnicar::maxJumpForce), fire(false), fireTime(1.0), IBallPicker(pi, plid), nWheels(nw)
29 {
30 carBid = new dBodyID[nw];
31 wheelBid = new dBodyID[nw];
32 wheelTrans = new ssgTransform* [nw];
33 carTrans = new ssgTransform* [nw];
34
35 charge = savedCharge = 3;
36
37 const double wheelRadius = 0.1;
38 sgVec3 zero = { 0.0, 0.0, 0.0 };
39
40
41 const double wheelDistance = carDim[1] + wheelRadius;
42
43 sgVec3 dirCenter = { 0.0, dirDim[1]/2, 0.0 };
44
45 // sgVec3 carCenter[3] = { { 1*carDim[1]/2, 0, 0 },
46 // { -0.5*carDim[1]/2, 1.7320508/2*carDim[1]/2, 0 },
47 // { -0.5*carDim[1]/2, -1.7320508/2*carDim[1]/2, 0 } };
48 //
49 // sgVec3 wheelCenter[3] = { { 1*wheelDistance, 0, 0 },
50 // { -0.5*wheelDistance, 1.7320508/2*wheelDistance, 0 },
51 // { -0.5*wheelDistance, -1.7320508/2*wheelDistance, 0 } };
52
53 dMass carMass, wheelMass, dirMass;
54 dGeomID * carGid = new dGeomID[nw];
55 dGeomID * wheelGid = new dGeomID[nw];
56
57 dJointID joe;
58
59 type = CAR;
60
61 jid = dJointGroupCreate(0);
62 dSpaceID sid = dSimpleSpaceCreate(c.getSid());
63 gid = (dGeomID) sid;
64 c.getScenePtr()->addKid(&trans);
65
66 ssgEntity* e;
67 if (plid <= 1) {
68 e = c.searchInPool("OmniCarBody1");
69 if (e == NULL) {
70 e = ssgLoadAC( "omnicar_1.ac" ) ;
71 e -> setName ("OmniCarBody1") ;
72 }
73 } else {
74 e = c.searchInPool("OmniCarBody2");
75 if (e == NULL) {
76 e = ssgLoadAC( "omnicar_2.ac" ) ;
77 e -> setName ("OmniCarBody2") ;
78 }
79 }
80
81 if (e != NULL) {
82 ssgFlatten ( e ) ;
83 ssgStripify( e ) ;
84 c.addToPool(e);
85 }
86
87 ssgTransform * carObj = new ssgTransform();
88 sgMat4 scalm;
89 sgMakeIdentMat4(scalm);
90 scalm[0][0] = carDim[0];
91 scalm[1][1] = carDim[1];
92 scalm[2][2] = carDim[2];
93 carObj->setTransform(scalm);
94 carObj->addKid(e);
95
96
97 ssgEntity* wheelObj = c.searchInPool("Wheel");
98 if (wheelObj == NULL) {
99 ssgaSphere* w = new ssgaSphere () ;
100 w -> setSize ( 2*wheelRadius ) ;
101 w -> setCenter ( zero ) ;
102 w -> setKidState ( c.getState(STA_CAR_WHEEL) ) ;
103 w -> regenerate () ;
104 w -> setName ("Wheel") ;
105 wheelObj = (ssgEntity*) w;
106 c.addToPool(wheelObj);
107 }
108
109 dMassSetBoxTotal ( &carMass, ODE_CAR_MASS, carDim[0], carDim[1], carDim[2]);
110 dMassSetSphereTotal ( &wheelMass, ODE_WHEEL_MASS, wheelRadius);
111
112 jam = new dJointID[nw];
113
114 for (int i = 0; i < nw; i++) {
115 dMatrix3 R;
116 double angle = ((double) i)*2.0/((double) nw)*M_PI;
117 carTrans[i] = new ssgTransform();
118 c.getScenePtr()->addKid(carTrans[i]);
119 carTrans[i]->addKid(carObj);
120 carBid[i] = dBodyCreate(c.getWid());
121 carGid[i] = dCreateBox (sid, carDim[0], carDim[1], carDim[2]);
122 dGeomSetBody (carGid[i], carBid[i]);
123 dGeomSetData (carGid[i], this);
124 dRFromAxisAndAngle (R, 0.0, 0.0, 1.0, angle+M_PI/2);
125 dBodySetRotation(carBid[i], R);
126 // dGeomSetPosition (carGid[i], (dReal) carCenter[i][0], (dReal) carCenter[i][1], (dReal) carCenter[i][2]);
127 dGeomSetPosition (carGid[i], (dReal) cos(angle)*carDim[1]/2.0, (dReal) sin(angle)*carDim[1]/2.0, 0.0);
128
129 dBodySetMass(carBid[i], &carMass);
130
131 wheelTrans[i] = new ssgTransform();
132 c.getScenePtr()->addKid(wheelTrans[i]);
133 wheelTrans[i]->addKid(wheelObj);
134 wheelBid[i] = dBodyCreate(c.getWid());
135 wheelGid[i] = dCreateSphere (sid, wheelRadius);
136 dGeomSetBody (wheelGid[i], wheelBid[i]);
137 dGeomSetData (wheelGid[i], this);
138 dGeomSetPosition (wheelGid[i], (dReal) cos(angle)*wheelDistance, (dReal) sin(angle)*wheelDistance, 0);
139 dBodySetMass(wheelBid[i], &wheelMass);
140 joe = dJointCreateBall (c.getWid(), jid);
141 dJointAttach (joe, carBid[i], wheelBid[i]);
142 dJointSetBallAnchor (joe, (dReal) cos(angle)*wheelDistance, (dReal) sin(angle)*wheelDistance, 0);
143
144 jam[i] = dJointCreateAMotor (c.getWid(), jid);
145 dJointAttach (jam[i], carBid[i], wheelBid[i]);
146 dJointSetAMotorMode (jam[i], dAMotorEuler);
147 dJointSetAMotorNumAxes (jam[i], 3);
148 dJointSetAMotorAxis (jam[i], 0, 1, 1.0, 0.0, 0.0);
149 dJointSetAMotorAxis (jam[i], 2, 2, 0.0, 0.0, 1.0);
150 }
151
152 for (int i = 1; i < nw; i++) {
153 joe = dJointCreateFixed (c.getWid(), jid);
154 dJointAttach (joe, carBid[i], carBid[i-1]);
155 dJointSetFixed(joe);
156 }
157 // joe = dJointCreateFixed (c.getWid(), jid);
158 // dJointAttach (joe, carBid[2], carBid[1]);
159 // dJointSetFixed(joe);
160
161 // dMassSetBoxTotal ( &dirMass, 0.1, carDim[0], carDim[1], carDim[2]);
162 dMassSetBoxTotal ( &dirMass, 0.1, dirDim[0], dirDim[1], dirDim[2]);
163
164 e = c.searchInPool("Picker");
165 if (e == NULL) {
166 e = ssgLoadAC( "picker.ac" ) ;
167 ssgFlatten ( e ) ;
168 ssgStripify( e ) ;
169 e -> setName ("Picker") ;
170 c.addToPool(e);
171 }
172
173 ssgTransform * dirObj = new ssgTransform();
174 sgMakeIdentMat4(scalm);
175 scalm[0][0] = dirDim[0];
176 scalm[1][1] = dirDim[1];
177 scalm[2][2] = dirDim[2];
178 dirObj->setTransform(scalm);
179 dirObj->addKid(e);
180
181 dirTrans = new ssgTransform();
182 c.getScenePtr()->addKid(dirTrans);
183 dirTrans->addKid(dirObj);
184
185 dirBid = dBodyCreate(c.getWid());
186 dBodySetMass(dirBid, &dirMass);
187 dBodySetPosition (dirBid, (dReal) dirCenter[0], (dReal) dirCenter[1], (dReal) dirCenter[2]);
188
189 dirGid = dCreateBox (sid, dirDim[0], dirDim[1], dirDim[2]);
190 dGeomSetBody (dirGid, dirBid);
191 dGeomSetData (dirGid, this);
192
193 joe = dJointCreateBall (c.getWid(), jid);
194 dJointAttach (joe, carBid[0], dirBid);
195 dJointSetBallAnchor (joe, (dReal) zero[0], (dReal) zero[1], (dReal) zero[2]);
196
197 diram = dJointCreateAMotor (c.getWid(), jid);
198 dJointAttach (diram, carBid[0], dirBid);
199 dJointSetAMotorMode (diram, dAMotorEuler);
200 dJointSetAMotorNumAxes (diram, 3);
201 dJointSetAMotorAxis (diram, 0, 2, 0.0, 1.0, 0.0);
202 dJointSetAMotorAxis (diram, 2, 1, 0.0, 0.0, 1.0);
203
204 dJointSetAMotorParam (diram, dParamLoStop, 0);
205 dJointSetAMotorParam (diram, dParamHiStop, 0);
206 dJointSetAMotorParam (diram, dParamLoStop2, 0);
207 dJointSetAMotorParam (diram, dParamHiStop2, 0);
208
209 }
210
isPickerGeom(dGeomID o)211 bool Omnicar::isPickerGeom( dGeomID o ) {
212 if ( o == dirGid)
213 return true;
214 return false;
215 }
216
~Omnicar()217 Omnicar::~Omnicar()
218 {
219 if (this->stickyBall)
220 this->context.releaseBall((IBallPicker*) this);
221 dJointGroupEmpty (jid);
222 for (int i=0; i<nWheels; i++) {
223 dBodyDestroy(wheelBid[i]);
224 dBodyDestroy(carBid[i]);
225 context.getScenePtr()->removeKid(wheelTrans[i]);
226 context.getScenePtr()->removeKid(carTrans[i]);
227 }
228 dBodyDestroy(dirBid);
229 context.getScenePtr()->removeKid(dirTrans);
230 }
231
releaseBall()232 void Omnicar::releaseBall() {
233 this->fireTime = 0;
234 this->stickyBall = false;
235 if (ballJoint != 0) {
236 dJointDestroy(ballJoint);
237 ballJoint = 0;
238 }
239 this->charge = this->savedCharge;
240
241 sgVec3 f;
242 sgNormalizeVec3(f, this->bodyDir);
243 sgScaleVec3(f, 100.0);
244 this->ball.addForce(f);
245 }
246
update(float dt)247 void Omnicar::update(float dt) {
248
249 sgVec3 normal; // direction normal to the ideal car plane
250 sgMat4 rot;
251 // getting the normal from the ssg node transform
252 // this->carTrans[0]->getTransform(rot);
253 // sgCopyVec3(normal, rot[2]);
254
255
256
257 dVector3 rotDir;
258 dJointGetAMotorAxis(diram, 2, rotDir);
259 normal[0] = (float) rotDir[0];
260 normal[1] = (float) rotDir[1];
261 normal[2] = (float) rotDir[2];
262 sgNormalizeVec3(normal);
263
264 // if (steering != 0) {
265 // sgMakeRotMat4(rot, steering*dt, normal);
266 // sgXformVec3(direction, rot);
267 // }
268
269 float steer = steering;
270 // since the car is double face we want the normal always point upward
271 if(sgScalarProductVec3(this->context.getGravity(), normal) > 0) {
272 // sgNegateVec3(normal);
273 // printf("Dio %f\n",dt);
274 steer = -steering;
275 }
276 // sgVec3 axis;
277 // sgVectorProductVec3(axis, normal, direction);
278 // sgNormalizeVec3(axis);
279 //
280 // sgVec3 normalDir;
281 // sgNormalizeVec3(normalDir, direction);
282 //
283 // float angle = 90 -180/M_PI*acos(sgScalarProductVec3(normalDir, normal));
284 // sgMakeRotMat4(rot, angle, axis);
285 // sgXformVec3(direction, rot);
286
287 dVector3 currentDir;
288 dJointGetAMotorAxis(diram, 0, currentDir);
289 bodyDir[0] = (float) currentDir[0];
290 bodyDir[1] = (float) currentDir[1];
291 bodyDir[2] = (float) currentDir[2];
292 sgNormalizeVec3(bodyDir);
293
294 // float bodyAngle = M_PI/2.0 - acos(sgScalarProductVec3(bodyDir, normalDir));
295 // float bodyAngularVel = bodyAngle/dt;
296 //
297 // printf("ANG %f\n", bodyAngle*180/M_PI);
298 //
299 // if (steering!=0) {
300 // bodyAngularVel -= steer;
301 // }
302 // dJointSetAMotorParam (diram, dParamVel3 , bodyAngularVel);
303 // dJointSetAMotorParam (diram, dParamFMax3, 20);
304
305 sgMat4 trans, res;
306 sgVec3 dirHpr;
307 sgVec3 dirPos;
308 const dReal* carPos;
309 sgVec3 ballDistance, temp;
310 const dReal* ballPos = dBodyGetPosition(this->ball.getBid());
311 this->getPos(dirPos);
312 sgScaleVec3(temp, bodyDir, dirDim[1] + this->ball.getSize()/2.0*1.1);
313
314 sgAddVec3(dirPos, temp);
315 ballDistance[0] = ballPos[0] - dirPos[0];
316 ballDistance[1] = ballPos[1] - dirPos[1];
317 ballDistance[2] = ballPos[2] - dirPos[2];
318
319
320 if (stickyBall && possessionIncrement!=0)
321 this->ball.setPossessionIncrement(possessionIncrement);
322
323 if (!stickyBall)
324 this->fireTime += dt;
325 if (sgLengthVec3(ballDistance) < this->ball.getSize()/1.7 && !stickyBall && (this->fireTime > 1.0)) {
326 this->context.releaseBall((IBallPicker*) this);
327 this->savedCharge = this->charge;
328 this->charge = 0;
329 stickyBall = true;
330 if (ballJoint != 0)
331 dJointDestroy(ballJoint);
332 ballJoint = dJointCreateBall (this->context.getWid(), 0);
333 // dJointSetFeedback (ballJoint, &jf);
334 dJointAttach (ballJoint, dirBid, this->ball.getBid());
335 dBodySetPosition(this->ball.getBid(), (double) dirPos[0], (double) dirPos[1], (double) dirPos[2]);
336 dJointSetBallAnchor (ballJoint, (double) dirPos[0], (double) dirPos[1], (double) dirPos[2]);
337 }
338 if (fire) {
339 this->fire = false;
340 if (stickyBall)
341 this->releaseBall();
342 this->charge = -this->charge;
343 }
344 if (speed != 0) {
345 // sgNormalizeVec3(direction);
346 // sgScaleVec3(direction, speed);
347 sgScaleVec3(bodyDir, speed);
348 }
349 if (steering != 0) {
350 // sgMakeRotMat4(rot, steering*dt, normal);
351 // sgXformVec3(direction, rot);
352 dJointSetAMotorParam (diram, dParamVel3 , -steer);
353 dJointSetAMotorParam (diram, dParamFMax3, 5);
354 }
355 else {
356 dJointSetAMotorParam (diram, dParamVel3 , 0);
357 dJointSetAMotorParam (diram, dParamFMax3, 5);
358 }
359
360 sgVec3 jumpDir;
361 if (jumping && (jumpForce > 0)) {
362 sgNormalizeVec3(jumpDir, this->context.getGravity());
363 sgVec3 jumpDirs;
364 // jumpForce -= 100*dt;
365 for(int i = 0; i<nWheels; i++){
366 sgScaleVec3(jumpDirs, jumpDir, -jumpForce*ODE_CAR_MASS);
367 dBodyAddForce(this->carBid[i], jumpDirs[0], jumpDirs[1], jumpDirs[2]);
368 sgScaleVec3(jumpDirs, jumpDir, -jumpForce*ODE_WHEEL_MASS);
369 dBodyAddForce(this->wheelBid[i], jumpDirs[0], jumpDirs[1], jumpDirs[2]);
370 }
371 if (stickyBall){
372 sgScaleVec3(jumpDirs, jumpDir, -jumpForce*ODE_BALL_MASS);
373 dBodyAddForce(this->ball.getBid(), jumpDirs[0], jumpDirs[1], jumpDirs[2]);
374 }
375 sgScaleVec3(jumpDirs, jumpDir, -jumpForce*0.1);
376 dBodyAddForce(this->dirBid, jumpDirs[0], jumpDirs[1], jumpDirs[2]);
377 }
378 else if (jumpForce < Omnicar::maxJumpForce) {
379 // jumpForce += 10*dt;
380 }
381 Entity::odeTrans2ssgTrans(this->dirBid, *(this->dirTrans));
382 sgMat4 mtx;
383 for (int i = 0; i < nWheels; i++) {
384 Entity::odeTrans2ssgTrans(this->carBid[i], *(this->carTrans[i]));
385 Entity::odeTrans2ssgTrans(this->wheelBid[i], *(this->wheelTrans[i]));
386 if (speed!=0){
387 if (!backwards)
388 dBodyAddForce(this->carBid[i], bodyDir[0], bodyDir[1], bodyDir[2]);
389 else
390 dBodyAddForce(this->carBid[i], -bodyDir[0], -bodyDir[1], -bodyDir[2]);
391 // if (!backwards)
392 // dBodyAddForce(this->carBid[i], direction[0], direction[1], direction[2]);
393 // else
394 // dBodyAddForce(this->carBid[i], -direction[0], -direction[1], -direction[2]);
395 dJointSetAMotorParam (jam[i], dParamFMax, 0);
396 dJointSetAMotorParam (jam[i], dParamFMax2, 0);
397 dJointSetAMotorParam (jam[i], dParamFMax3, 0);
398 }
399 else {
400 dJointSetAMotorParam (jam[i], dParamVel, 0.01);
401 dJointSetAMotorParam (jam[i], dParamFMax, 5);
402 dJointSetAMotorParam (jam[i], dParamVel2, 0.01);
403 dJointSetAMotorParam (jam[i], dParamFMax2, 5);
404 dJointSetAMotorParam (jam[i], dParamVel3, 0.01);
405 dJointSetAMotorParam (jam[i], dParamFMax3, 5);
406 }
407
408 }
409
410 /* printf("angle: %f\n", angle);
411 printf("normal: (%f) %f %f %f\n", sgLengthVec3(normal), normal[0], normal[1], normal[2]);
412 printf("axis: (%f) %f %f %f\n", sgLengthVec3(axis), axis[0], axis[1], axis[2]);
413 printf("direction: (%f) %f %f %f\n", sgLengthVec3(direction), direction[0], direction[1], direction[2]);*/
414 }
415
getPos(sgVec3 out)416 void Omnicar::getPos(sgVec3 out)
417 {
418 const dReal* carPos;
419 sgZeroVec3(out);
420 for (int i = 0; i<nWheels; i++){
421 carPos = dBodyGetPosition(carBid[i]);
422 out[0] += carPos[0];
423 out[1] += carPos[1];
424 out[2] += carPos[2];
425 }
426 sgScaleVec3(out, 1.0/((float) nWheels));
427 }
428
traslateBody(dBodyID b,dVector3 diff)429 void Omnicar::traslateBody(dBodyID b, dVector3 diff) {
430 const dReal * bodyPos = dBodyGetPosition(b);
431 dBodySetLinearVel (b, 0, 0, 0);
432 dBodySetAngularVel (b, 0, 0, 0);
433 dBodySetPosition(b, bodyPos[0]+diff[0], bodyPos[1]+diff[1], bodyPos[2]+diff[2]);
434 }
435
setPos(double x,double y,double z)436 void Omnicar::setPos(double x, double y, double z)
437 {
438 sgVec3 currPos;
439 this->getPos(currPos);
440 dVector3 diff;
441
442 diff[0] = (dReal) x - currPos[0];
443 diff[1] = (dReal) y - currPos[1];
444 diff[2] = (dReal) z - currPos[2];
445
446 for (int i = 0; i < nWheels; i++){
447 Omnicar::traslateBody(carBid[i], diff);
448 Omnicar::traslateBody(wheelBid[i], diff);
449
450 }
451 Omnicar::traslateBody(dirBid, diff);
452 // Omnicar::traslateBody(rejBid, diff);
453
454 // for (int i=0; i<2; i++)
455 // Omnicar::traslateBody(borBid[i], diff);
456
457 }
458
getMagnetDir(sgVec3 out)459 void Omnicar::getMagnetDir(sgVec3 out) {
460 sgNormalizeVec3(out, bodyDir);
461 }
462
addForce(sgVec3 force)463 void Omnicar::addForce(sgVec3 force) {
464 for (int i = 0; i < nWheels; i++)
465 dBodyAddForce(this->carBid[i], force[0], force[1], force[2]);
466 }
467
getMagnetPos(sgVec3 out)468 void Omnicar::getMagnetPos(sgVec3 out)
469 {
470 sgVec3 ndir;
471 this->getPos(out);
472 // sgNormalizeVec3(ndir, direction);
473 sgNormalizeVec3(ndir, bodyDir);
474 sgScaleVec3(ndir, dirDim[1] + this->ball.getSize()/2.0);
475 sgAddVec3(out, ndir);
476 }
477
move(Entity::Move m)478 void Omnicar::move(Entity::Move m)
479 {
480 switch (m) {
481 case GO_FWRD:
482 speed = 100;
483 backwards = false;
484 break;
485 case GO_BWRD:
486 speed = 120;
487 backwards = true;
488 break;
489 case TURN_LEFT:
490 steering = 5;
491 break;
492 case TURN_RIGHT:
493 steering = -5;
494 break;
495 case FIRE:
496 this->fire = true;
497 this->context.sound.play(Sound::FIRE);
498 break;
499 case JUMP:
500 printf("boing!\n");
501 // this->context.sound.play(Sound::FIRE);
502 jumping = true;
503 break;
504 case GO_STOP:
505 backwards = false;
506 speed = 0;
507 break;
508 case TURN_STOP:
509 steering = 0;
510 break;
511 case JUMP_STOP:
512 printf("being!\n");
513 jumping = false;
514 break;
515 default:
516 printf("Warning: unimplemented movement in %s.", this->toString());
517 break;
518 }
519 }
520
521
522 };
523