00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "vpdof.h"
00031 #include "vpjoint.h"
00032 #include "vpmodifier.h"
00033 #include <cmath>
00034
00035 using namespace std;
00036
00037 VPDof::VPDof()
00038 {
00039 ownerJoint = NULL;
00040 evoluta = NULL;
00041 rangeModifier = NULL;
00042
00043
00044
00045
00046 axis.SetXYZW(0,0,1,0);
00047 position.SetXYZW(0,0,0,1);
00048 lim.MakeIdentity();
00049
00050
00051 minAngle = 0;
00052 maxAngle = 0;
00053 currentPosition = 0;
00054 restPosition = 0;
00055 }
00056
00057 VPDof::VPDof(const VPDof& dof)
00058 {
00059 description = dof.description;
00060 position = dof.position;
00061 axis = dof.axis;
00062 evoluta = dof.evoluta;
00063 lim = dof.lim;
00064 minAngle = dof.minAngle;
00065 maxAngle = dof.maxAngle;
00066 currentMinAngle = dof.currentMinAngle;
00067 currentMaxAngle = dof.currentMaxAngle;
00068 confortMinAngle = dof.confortMinAngle;
00069 confortMaxAngle = dof.confortMaxAngle;
00070 rangeModifier = dof.rangeModifier;
00071 currentPosition = dof.currentPosition;
00072 restPosition = dof.restPosition;
00073 ownerJoint = dof.ownerJoint;
00074 }
00075
00076 VPDof::VPDof(const VPPoint4D& vec, const VPPoint4D& pos, float min, float max)
00077 {
00078 ownerJoint = NULL;
00079 evoluta = NULL;
00080 rangeModifier = NULL;
00081 axis = vec;
00082 position = pos;
00083 minAngle = min;
00084 currentMinAngle = min;
00085 maxAngle = max;
00086 currentMaxAngle = max;
00087 currentPosition = (0-min)/(max-min);
00088 ComputeLIM();
00089 }
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127 VPDof& VPDof::operator=(const VPDof& dof)
00128 {
00129 description = dof.description;
00130 position = dof.position;
00131 axis = dof.axis;
00132 evoluta = dof.evoluta;
00133 lim = dof.lim;
00134 minAngle = dof.minAngle;
00135 maxAngle = dof.maxAngle;
00136 currentMinAngle = dof.currentMinAngle;
00137 currentMaxAngle = dof.currentMaxAngle;
00138 confortMinAngle = dof.confortMinAngle;
00139 confortMaxAngle = dof.confortMaxAngle;
00140 rangeModifier = dof.rangeModifier;
00141 currentPosition = dof.currentPosition;
00142 restPosition = dof.restPosition;
00143 ownerJoint = dof.ownerJoint;
00144 return *this;
00145 }
00146
00147 void VPDof::Set(const VPPoint4D& vec, const VPPoint4D& pos, float min, float max)
00148 {
00149 axis = vec;
00150 position = pos;
00151 minAngle = min;
00152 currentMinAngle = min;
00153 maxAngle = max;
00154 currentMaxAngle = max;
00155
00156
00157 currentPosition = (0-min)/(max-min);
00158
00159 ComputeLIM();
00160 }
00161
00162 void VPDof::SetDescription(const string& desc)
00163 {
00164 description = desc;
00165 }
00166
00167 VPPoint4D VPDof::GetAxis()
00168 {
00169 return axis;
00170 }
00171
00172 VPPoint4D VPDof::GetOrigin()
00173 {
00174 VPPoint4D result;
00175 lim.GetTranslation(&result);
00176 return result;
00177 }
00178
00179 VPTransform VPDof::GetLim()
00180 {
00181 return lim;
00182 }
00183
00184 void VPDof::GetLim(VPTransform* ptrResult)
00185 {
00186 *ptrResult = lim;
00187 }
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229 float VPDof::GetMin()
00230 {
00231 return minAngle;
00232 }
00233
00234 float VPDof::GetMax()
00235 {
00236 return maxAngle;
00237 }
00238
00239 float VPDof::GetCurrentMin()
00240 {
00241 float minModif = minAngle;
00242 if (rangeModifier)
00243 minModif = rangeModifier->GetMin();
00244 if (minAngle < minModif)
00245 return minModif;
00246 return minAngle;
00247 }
00248
00249 float VPDof::GetCurrentMax()
00250 {
00251 float maxModif = maxAngle;
00252 if (rangeModifier)
00253 maxModif = rangeModifier->GetMax();
00254 if (maxAngle > maxModif)
00255 return maxModif;
00256 return maxAngle;
00257 }
00258
00259 float VPDof::GetCurrent()
00260 {
00261 return currentPosition;
00262 }
00263
00264 float VPDof::GetRest()
00265 {
00266 return restPosition;
00267 }
00268
00269 VPJoint* VPDof::GetOwnerJoint()
00270 {
00271 return ownerJoint;
00272 }
00273
00274 void VPDof::SetAxis(VPVector3D za)
00275 {
00276 axis = za;
00277 }
00278
00279 void VPDof::SetEvoluta(VPBezier* evol)
00280 {
00281 evoluta = evol;
00282 }
00283
00284
00285
00286 void VPDof::SetLim(const VPTransform& t)
00287 {
00288 lim = t;
00289 }
00290
00291 void VPDof::SetMin(float min)
00292 {
00293 minAngle = min;
00294 }
00295
00296 void VPDof::SetMax(float max)
00297 {
00298 maxAngle = max;
00299 }
00300
00301 void VPDof::MoveTo(float pos)
00302 {
00303
00304 if (pos > 1.0) pos = 1.0;
00305 if (pos < 0.0) pos = 0.0;
00306
00307
00308 double newAngle = currentMinAngle + pos * (currentMaxAngle - currentMinAngle);
00309 double currentAngle = currentMinAngle + currentPosition * (currentMaxAngle - currentMinAngle);
00310
00311
00312 VPPoint4D center = position;
00313 if (evoluta)
00314 {
00315 VPPoint4D bezierPoint;
00316 evoluta->GetPoint(currentPosition, &bezierPoint);
00317 bezierPoint.SetW(0);
00318 center = position + bezierPoint;
00319 }
00320
00321
00322 VPTransform diffTrans;
00323 diffTrans.MakeRotation(center, axis, currentAngle-newAngle);
00324 ownerJoint->TransformDofs(this, diffTrans);
00325
00326
00327 currentPosition = pos;
00328 lim.MakeRotation(center, axis, newAngle);
00329
00330
00331 ownerJoint->MakeLim();
00332 }
00333
00334 void VPDof::ComputeLIM()
00335 {
00336
00337 double angle = currentMinAngle + currentPosition * (currentMaxAngle - currentMinAngle);
00338
00339
00340 VPPoint4D center;
00341 if (evoluta)
00342 {
00343 VPPoint4D bezierPoint;
00344 evoluta->GetPoint(currentPosition, &bezierPoint);
00345 bezierPoint.SetW(0);
00346 center = position + bezierPoint;
00347 }
00348 else
00349 center = position;
00350
00351 lim.MakeRotation(center, axis, angle);
00352 }
00353
00354 void VPDof::Transform(const VPTransform& t)
00355 {
00356 static unsigned int transformCount = 0;
00357 VPPoint4D center;
00358 t.ApplyTo(&position);
00359 t.ApplyTo(&axis);
00360
00361
00362 if (++transformCount > 50)
00363 {
00364 axis.Normalize();
00365 position.Normalize();
00366 transformCount = 0;
00367 }
00368
00369 if (evoluta)
00370 {
00371 evoluta->Transform(t);
00372 VPPoint4D bezierPoint;
00373 evoluta->GetPoint(currentPosition, &bezierPoint);
00374 bezierPoint.SetW(0);
00375 center = position + bezierPoint;
00376 }
00377 else
00378 center = position;
00379 double angle = currentMinAngle + currentPosition * (currentMaxAngle - currentMinAngle);
00380 lim.MakeRotation(center, axis, angle);
00381 }
00382
00383 void VPDof::SetOwnerJoint(VPJoint* ow)
00384 {
00385 ownerJoint = ow;
00386 }
00387
00388 void VPDof::SetRest(float rest)
00389 {
00390 restPosition = rest;
00391 }
00392
00393 void VPDof::Rest()
00394 {
00395 MoveTo(restPosition);
00396 }
00397
00398 void VPDof::SetRangeModifier(VPModifier* m)
00399 {
00400 rangeModifier = m;
00401 }
00402
00403 VPModifier* VPDof::GetRangeModifier()
00404 {
00405 return rangeModifier;
00406 }
00407
00408 void VPDof::ApplyTransformTo(VPTransform* ptrTrans) const
00409 {
00410 *ptrTrans = lim * (*ptrTrans);
00411 }