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 "goalkeeper.h"
22
23 namespace gillo {
24
25 const double Goalkeeper::weight = 10;
26 const double Goalkeeper::radius = 0.5;
27 const double Goalkeeper::operativeRadius = ODE_GOAL_DISTANCE/2*1.5;
28 const double Goalkeeper::aimingRadius = operativeRadius * 4;
29 const double Goalkeeper::posCorrIntensity = 1000;
30 const double Goalkeeper::rotCorrIntensity = 300;
31 const float Goalkeeper::maxMagnetIntensity = 500;
32
33
Goalkeeper(Context & c,Goal & g,SimpleBall & b)34 Goalkeeper::Goalkeeper(Context& c, Goal& g, SimpleBall& b)
35 : Entity(c), goal(g), ball(b)
36 {
37 dMass mass;
38 sgVec3 dim = { 2.2*radius, 0, 2.2*radius };
39 sgVec3 center = { 0, 0, 0};
40
41 type = GOALKEEPER;
42 bid = dBodyCreate(c.getWid());
43 gid = dCreateSphere (c.getSid(), radius);
44 Entity::Init();
45
46 ssgEntity* e = c.searchInPool("Goalkeeper");
47 if (e == NULL) {
48 ssgaSphere* e_obj = new ssgaSphere () ;
49 e_obj -> setSize ( 2*radius ) ;
50 e_obj -> setCenter ( center ) ;
51 e_obj -> setKidState ( c.getState(STA_GOALKEEPER) ) ;
52 e_obj -> regenerate () ;
53 e_obj -> setName ("Goalkeeper") ;
54 e = (ssgEntity*) e_obj;
55 c.addToPool(e);
56 }
57 trans.addKid( e );
58
59 // e = c.searchInPool("GoalkeeperFur");
60 // if (e == NULL) {
61 // ssgaCube * e_obj = new ssgaCube () ;
62 // e_obj -> setSize ( dim ) ;
63 // e_obj -> setCenter ( center ) ;
64 // e_obj -> setKidState ( c.getState(STA_GOALKEEPERFUR) ) ;
65 // e_obj -> regenerate () ;
66 // e_obj->setName("GoalkeeperFur");
67 // e = (ssgEntity*) e_obj;
68 // c.addToPool(e);
69 // }
70 // ssgCutout* co = new ssgCutout(true);
71 // ssgTransform* tr0 = new ssgTransform();
72 // ssgTransform* tr1 = new ssgTransform();
73 // tr0->addKid(e);
74 // tr1->addKid(e);
75 // co->addKid(tr0);
76 // co->addKid(tr1);
77 // alphaTrans.addKid( co );
78
79 dMassSetSphereTotal(&mass, weight, radius);
80 // dMassTranslate (&mass, 0, 0, 2*radius);
81 dBodySetMass(bid, &mass);
82 dBodySetPosition(bid, 0, 0, 0);
83
84 jid = dJointGroupCreate(0);
85 jam = dJointCreateAMotor (c.getWid(), jid);
86 dJointAttach (jam, this->bid, 0);
87 dJointSetAMotorMode (jam, dAMotorEuler);
88 dJointSetAMotorNumAxes (jam, 3);
89 dJointSetAMotorAxis (jam, 0, 2, 0.0, 1.0, 0.0);
90 dJointSetAMotorAxis (jam, 2, 1, 0.0, 0.0, 1.0);
91 dJointSetAMotorParam (jam, dParamLoStop, 0);
92 dJointSetAMotorParam (jam, dParamHiStop, 0);
93 dJointSetAMotorParam (jam, dParamLoStop2, 0);
94 dJointSetAMotorParam (jam, dParamHiStop2, 0);
95 }
96
97
~Goalkeeper()98 Goalkeeper::~Goalkeeper()
99 {
100 }
101
update(float dt)102 void Goalkeeper::update(float dt) {
103 sgVec3 myPos, ballPos, diff, normGravity;
104 float currentDistance, distance, ballDistance;
105 sgMat4 rot;
106
107 // roll control
108 // this->trans.getTransform(rot);
109 // sgNegateVec3(rot[2]);
110 // sgNormalizeVec3(normGravity, context.getGravity());
111 // sgSubVec3(diff, normGravity, rot[2]);
112 // distance = sgLengthVec3(diff);
113 // if (distance > 0) {
114 // sgScaleVec3(diff, rotCorrIntensity);
115 // dBodyAddForceAtRelPos(this->bid, diff[0], diff[1], diff[2], 0,0,-radius);
116 // }
117
118
119
120 this->getPos(myPos);
121 currentDistance = -dGeomCCylinderPointDepth (goal.getAreaGeom(), (dReal) myPos[0], (dReal) myPos[1], (dReal) myPos[2]) + goal.getAreaRadius();
122
123 ball.getPos(ballPos);
124 sgSubVec3(diff, ballPos, myPos);
125
126 // scope limits
127 ballDistance = -dGeomCCylinderPointDepth (goal.getAreaGeom(), (dReal) ballPos[0], (dReal) ballPos[1], (dReal) ballPos[2]) + goal.getAreaRadius();
128
129 if (currentDistance > operativeRadius || ballDistance > operativeRadius ){
130 if (ballDistance < aimingRadius) {
131 // track Z position...
132 myPos[2] = -diff[2];
133 }
134 sgScaleVec3(myPos, posCorrIntensity/operativeRadius);
135 dBodyAddForce(this->bid, -myPos[0], -myPos[1], -myPos[2]);
136 if (ballDistance > operativeRadius) {
137 const dReal * myVel = dBodyGetLinearVel(this->bid);
138 dBodySetLinearVel(this->bid, myVel[0]*0.8, myVel[1]*0.8, myVel[2]*0.8);
139 }
140 this->charge = 0;
141 // printf("CD: %f BD: %f (%f)\n", currentDistance, ballDistance, currentDistance*(posCorrIntensity/operativeRadius));
142 }
143 else {
144 this->charge = 10000;
145 }
146
147 // catch the ball
148 float l = sgLengthVec3(diff);
149 sgNormalizeVec3(diff);
150 float intensity = this->charge*10/l/l;
151 if (sgAbs(intensity) > maxMagnetIntensity)
152 intensity = intensity > 0 ? maxMagnetIntensity : -maxMagnetIntensity;
153 sgScaleVec3(diff, intensity);
154 dBodyAddForce(this->bid, diff[0]*weight, diff[1]*weight, diff[2]*weight);
155
156 // if (ballDistance < goal.getAreaRadius()*0.8) {
157 // sgNegateVec3(diff);
158 // sgScaleVec3(diff, ODE_BALL_MASS);
159 // ball.addForce(diff);
160 // }
161
162 // update roto-translations
163 // static float i = 0, angle = 0;
164 // angle += 2*dt;
165 // i += dt;
166 // Entity::transform(((ssgTransform*) ((ssgCutout*) alphaTrans.getKid(0))->getKid(0)), angle, 0.05*cos(i/4)+0.95);
167 // Entity::transform(((ssgTransform*) ((ssgCutout*) alphaTrans.getKid(0))->getKid(1)), -0.2*angle, 0.15*cos(i/8)+0.9);
168
169 Entity::odeTrans2ssgTrans(this->bid, this->trans);
170 sgMat4 xform;
171 trans.getTransform(xform);
172 alphaTrans.setTransform(xform);
173 }
174
move(Move m)175 void Goalkeeper::move(Move m) {
176 // should not be controlled
177 return;
178 }
179
180
181 };
182