2 #include "BodyEnvironment.hpp"
3 #include "RigidBody.hpp"
12 _pos[i] = _cmpr[i] = 0;
24 _global_ground[i] = _global_vel[i] = 0;
25 _global_ground[2] = 1;
26 _global_ground[3] = -1e3;
29 void Gear::setPosition(float* position)
32 for(i=0; i<3; i++) _pos[i] = position[i];
35 void Gear::setCompression(float* compression)
38 for(i=0; i<3; i++) _cmpr[i] = compression[i];
41 void Gear::setSpring(float spring)
46 void Gear::setDamping(float damping)
51 void Gear::setStaticFriction(float sfric)
56 void Gear::setDynamicFriction(float dfric)
61 void Gear::setBrake(float brake)
63 _brake = Math::clamp(brake, 0, 1);
66 void Gear::setRotation(float rotation)
71 void Gear::setExtension(float extension)
73 _extension = Math::clamp(extension, 0, 1);
76 void Gear::setCastering(bool c)
81 void Gear::setGlobalGround(double *global_ground, float* global_vel)
84 for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
85 for(i=0; i<3; i++) _global_vel[i] = global_vel[i];
88 void Gear::getPosition(float* out)
91 for(i=0; i<3; i++) out[i] = _pos[i];
94 void Gear::getCompression(float* out)
97 for(i=0; i<3; i++) out[i] = _cmpr[i];
100 void Gear::getGlobalGround(double* global_ground)
103 for(i=0; i<4; i++) global_ground[i] = _global_ground[i];
106 float Gear::getSpring()
111 float Gear::getDamping()
116 float Gear::getStaticFriction()
121 float Gear::getDynamicFriction()
126 float Gear::getBrake()
131 float Gear::getRotation()
136 float Gear::getExtension()
141 void Gear::getForce(float* force, float* contact)
143 Math::set3(_force, force);
144 Math::set3(_contact, contact);
152 float Gear::getCompressFraction()
157 bool Gear::getCastering()
162 void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
164 // Init the return values
166 for(i=0; i<3; i++) _force[i] = _contact[i] = 0;
168 // Don't bother if it's not down
172 // The ground plane transformed to the local frame.
174 s->planeGlobalToLocal(_global_ground, ground);
176 // The velocity of the contact patch transformed to local coordinates.
178 s->velGlobalToLocal(_global_vel, glvel);
180 // First off, make sure that the gear "tip" is below the ground.
181 // If it's not, there's no force.
182 float a = ground[3] - Math::dot3(_pos, ground);
193 // Now a is the distance from the tip to ground, so make b the
194 // distance from the base to ground. We can get the fraction
195 // (0-1) of compression from a/(a-b). Note the minus sign -- stuff
196 // above ground is negative.
198 Math::add3(_cmpr, _pos, tmp);
199 float b = ground[3] - Math::dot3(tmp, ground);
201 // Calculate the point of ground _contact.
205 _contact[i] = _pos[i] + _frac*_cmpr[i];
207 // Turn _cmpr into a unit vector and a magnitude
209 float clen = Math::mag3(_cmpr);
210 Math::mul3(1/clen, _cmpr, cmpr);
212 // Now get the velocity of the point of contact
214 body->pointVelocity(_contact, rot, cv);
215 Math::add3(cv, v, cv);
216 Math::sub3(cv, glvel, cv);
218 // Finally, we can start adding up the forces. First the spring
219 // compression. (note the clamping of _frac to 1):
220 _frac = (_frac > 1) ? 1 : _frac;
221 float fmag = _frac*clen*_spring;
223 // Then the damping. Use the only the velocity into the ground
224 // (projection along "ground") projected along the compression
225 // axis. So Vdamp = ground*(ground dot cv) dot cmpr
226 Math::mul3(Math::dot3(ground, cv), ground, tmp);
227 float dv = Math::dot3(cmpr, tmp);
228 float damp = _damp * dv;
229 if(damp > fmag) damp = fmag; // can't pull the plane down!
230 if(damp < -fmag) damp = -fmag; // sanity
232 // The actual force applied is only the component perpendicular to
233 // the ground. Side forces come from velocity only.
234 _wow = (fmag - damp) * -Math::dot3(cmpr, ground);
235 Math::mul3(-_wow, ground, _force);
237 // Wheels are funky. Split the velocity along the ground plane
238 // into rolling and skidding components. Assuming small angles,
239 // we generate "forward" and "left" unit vectors (the compression
240 // goes "up") for the gear, make a "steer" direction from these,
241 // and then project it onto the ground plane. Project the
242 // velocity onto the ground plane too, and extract the "steer"
243 // component. The remainder is the skid velocity.
245 float gup[3]; // "up" unit vector from the ground
246 Math::set3(ground, gup);
247 Math::mul3(-1, gup, gup);
249 float xhat[] = {1,0,0};
250 float steer[3], skid[3];
251 Math::cross3(gup, xhat, skid); // up cross xhat =~ skid
252 Math::unit3(skid, skid); // == skid
254 Math::cross3(skid, gup, steer); // skid cross up == steer
257 // Correct for a rotation
258 float srot = Math::sin(_rot);
259 float crot = Math::cos(_rot);
262 steer[0] = crot*tx + srot*ty;
263 steer[1] = -srot*tx + crot*ty;
267 skid[0] = crot*tx + srot*ty;
268 skid[1] = -srot*tx + crot*ty;
271 float vsteer = Math::dot3(cv, steer);
272 float vskid = Math::dot3(cv, skid);
273 float wgt = Math::dot3(_force, gup); // force into the ground
276 _rollSpeed = Math::sqrt(vsteer*vsteer + vskid*vskid);
277 // Don't modify caster angle when the wheel isn't moving,
278 // or else the angle will animate the "jitter" of a stopped
280 if(_rollSpeed > 0.05)
281 _casterAngle = Math::atan2(vskid, vsteer);
288 float fsteer = _brake * calcFriction(wgt, vsteer);
289 float fskid = calcFriction(wgt, vskid);
290 if(vsteer > 0) fsteer = -fsteer;
291 if(vskid > 0) fskid = -fskid;
293 // Phoo! All done. Add it up and get out of here.
294 Math::mul3(fsteer, steer, tmp);
295 Math::add3(tmp, _force, _force);
297 Math::mul3(fskid, skid, tmp);
298 Math::add3(tmp, _force, _force);
301 float Gear::calcFriction(float wgt, float v)
303 // How slow is stopped? 10 cm/second?
304 const float STOP = 0.1f;
305 const float iSTOP = 1.0f/STOP;
307 if(v < STOP) return v*iSTOP * wgt * _sfric;
308 else return wgt * _dfric;
311 }; // namespace yasim