2 #include "RigidBody.hpp"
11 _pos[i] = _cmpr[i] = 0;
21 void Gear::setPosition(float* position)
24 for(i=0; i<3; i++) _pos[i] = position[i];
27 void Gear::setCompression(float* compression)
30 for(i=0; i<3; i++) _cmpr[i] = compression[i];
33 void Gear::setSpring(float spring)
38 void Gear::setDamping(float damping)
43 void Gear::setStaticFriction(float sfric)
48 void Gear::setDynamicFriction(float dfric)
53 void Gear::setBrake(float brake)
55 _brake = Math::clamp(brake, 0, 1);
58 void Gear::setRotation(float rotation)
63 void Gear::setExtension(float extension)
65 _extension = Math::clamp(extension, 0, 1);
68 void Gear::getPosition(float* out)
71 for(i=0; i<3; i++) out[i] = _pos[i];
74 void Gear::getCompression(float* out)
77 for(i=0; i<3; i++) out[i] = _cmpr[i];
80 float Gear::getSpring()
85 float Gear::getDamping()
90 float Gear::getStaticFriction()
95 float Gear::getDynamicFriction()
100 float Gear::getBrake()
105 float Gear::getRotation()
110 float Gear::getExtension()
115 void Gear::getForce(float* force, float* contact)
117 Math::set3(_force, force);
118 Math::set3(_contact, contact);
126 float Gear::getCompressFraction()
131 void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
133 // Init the return values
135 for(i=0; i<3; i++) _force[i] = _contact[i] = 0;
137 // Don't bother if it's not down
143 // First off, make sure that the gear "tip" is below the ground.
144 // If it's not, there's no force.
145 float a = ground[3] - Math::dot3(_pos, ground);
152 // Now a is the distance from the tip to ground, so make b the
153 // distance from the base to ground. We can get the fraction
154 // (0-1) of compression from a/(a-b). Note the minus sign -- stuff
155 // above ground is negative.
156 Math::add3(_cmpr, _pos, tmp);
157 float b = ground[3] - Math::dot3(tmp, ground);
159 // Calculate the point of ground _contact.
163 _contact[i] = _pos[i] + _frac*_cmpr[i];
165 // Turn _cmpr into a unit vector and a magnitude
167 float clen = Math::mag3(_cmpr);
168 Math::mul3(1/clen, _cmpr, cmpr);
170 // Now get the velocity of the point of contact
172 body->pointVelocity(_contact, rot, cv);
173 Math::add3(cv, v, cv);
175 // Finally, we can start adding up the forces. First the spring
176 // compression. (note the clamping of _frac to 1):
177 _frac = (_frac > 1) ? 1 : _frac;
178 float fmag = _frac*clen*_spring;
180 // Then the damping. Use the only the velocity into the ground
181 // (projection along "ground") projected along the compression
182 // axis. So Vdamp = ground*(ground dot cv) dot cmpr
183 Math::mul3(Math::dot3(ground, cv), ground, tmp);
184 float dv = Math::dot3(cmpr, tmp);
185 float damp = _damp * dv;
186 if(damp > fmag) damp = fmag; // can't pull the plane down!
187 if(damp < -fmag) damp = -fmag; // sanity
189 // The actual force applied is only the component perpendicular to
190 // the ground. Side forces come from velocity only.
191 _wow = (fmag - damp) * -Math::dot3(cmpr, ground);
192 Math::mul3(-_wow, ground, _force);
194 // Wheels are funky. Split the velocity along the ground plane
195 // into rolling and skidding components. Assuming small angles,
196 // we generate "forward" and "left" unit vectors (the compression
197 // goes "up") for the gear, make a "steer" direction from these,
198 // and then project it onto the ground plane. Project the
199 // velocity onto the ground plane too, and extract the "steer"
200 // component. The remainder is the skid velocity.
202 float gup[3]; // "up" unit vector from the ground
203 Math::set3(ground, gup);
204 Math::mul3(-1, gup, gup);
206 float xhat[] = {1,0,0};
207 float steer[3], skid[3];
208 Math::cross3(gup, xhat, skid); // up cross xhat =~ skid
209 Math::unit3(skid, skid); // == skid
211 Math::cross3(skid, gup, steer); // skid cross up == steer
214 // Correct for a (small) rotation
215 Math::mul3(_rot, steer, tmp);
216 Math::add3(tmp, skid, skid);
217 Math::unit3(skid, skid);
218 Math::cross3(skid, gup, steer);
221 float vsteer = Math::dot3(cv, steer);
222 float vskid = Math::dot3(cv, skid);
223 float wgt = Math::dot3(_force, gup); // force into the ground
225 float fsteer = _brake * calcFriction(wgt, vsteer);
226 float fskid = calcFriction(wgt, vskid);
227 if(vsteer > 0) fsteer = -fsteer;
228 if(vskid > 0) fskid = -fskid;
230 // Phoo! All done. Add it up and get out of here.
231 Math::mul3(fsteer, steer, tmp);
232 Math::add3(tmp, _force, _force);
234 Math::mul3(fskid, skid, tmp);
235 Math::add3(tmp, _force, _force);
238 float Gear::calcFriction(float wgt, float v)
240 // How slow is stopped? 50 cm/second?
241 const float STOP = 0.5;
242 const float iSTOP = 1/STOP;
244 if(v < STOP) return v*iSTOP * wgt * _sfric;
245 else return wgt * _dfric;
248 }; // namespace yasim