Main Page | Namespace List | Class Hierarchy | Class List | Directories | File List | Class Members | File Members | Related Pages

vpdof.cpp

Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 // ChangeLog
00006 // Jun 02, 2005 - Bruno de Oliveira Schneider
00007 // - VPDof(const VPPoint4D&, const VPPoint4D&, float, float) now computes LIM.
00008 // - GetDescription is now inline and returns by reference.
00009 // - DOFs are now derived from memory objects. The programmer no longer needs to keep track
00010 //   of all DOFs in a body.
00011 // - Added some documentation.
00012 // Oct 08, 2004 - Bruno de Oliveira Schneider
00013 // - Code typing has been cleaned (tabs replaced by spaces, excessive spaces
00014 //   removed, etc.) because tabs and spaces were mixed together.
00015 // - Removed "#include <stdio.h>". Removed "PrintLim" method.
00016 // - Removed "vp" prefix from every method name.
00017 // - Moved "using namespace std" from header file to implementation file.
00018 // - Changed type of "lim" attribute from "VPMatrix" to "VPTransform".
00019 // - Changed type of "axis" and "position" attributes to VPPoint4D.
00020 // - Changed "SetDescription" to avoid passing a string by value and removed the
00021 //   "char*" version because it is not needed.
00022 // - Changed "GetEvoluta".
00023 // - Commented out "bim" attribute which was never used.
00024 // - Commented out "GetBim()".
00025 // - Added "Move" (inline) and "Transform(const VPTransform&)" methods.
00026 // - Added copy constructor and operator=.
00027 // Feb 23, 2001 - Anderson Maciel
00028 // - Class and new methods declaration.
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     // These initializations are needed in order to make a joint automatically compute
00044     // its transform when setting dofs. They could be removed if the application programmer
00045     // is forced to tell when a joint is ready to compute its transform.
00046     axis.SetXYZW(0,0,1,0);
00047     position.SetXYZW(0,0,0,1);
00048     lim.MakeIdentity();
00049 
00050     // FixMe: (by Bruno) Not sure if the folowing initializations are needed...
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; //FixMe: Maybe a new evoluta should be created
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 //~ VPDof::VPDof(VPVector3D za, VPPoint3D pos, VPBezier* evol) {
00092     //~ axis = za;
00093     //~ evoluta = evol;
00094     //~ position = pos;
00095     //~ lim = VPMatrix();
00096     //~ minAngle = 0;
00097     //~ maxAngle = 0;
00098     //~ currentPosition = 0;
00099     //~ restPosition = 0;
00100     //~ rangeModifier = NULL;
00101 //~ }
00102 
00103 //~ VPDof::VPDof(VPVector3D za, VPPoint3D pos, VPCurve* evol, float min, float max, float cur, float rest) {
00104     //~ axis = za;
00105     //~ evoluta = (VPBezier*)evol;
00106     //~ position = pos;
00107     //~ lim = VPMatrix();
00108     //~ minAngle = min;
00109     //~ maxAngle = max;
00110     //~ currentPosition = cur;
00111     //~ restPosition = rest;
00112     //~ rangeModifier = NULL;
00113 //~ }
00114 
00115 //~ VPDof::VPDof(VPVector3D za, VPPoint3D pos, VPBezier* evol, float min, float max, float cur, float rest, VPModifier* rm) {
00116     //~ axis = za;
00117     //~ evoluta = evol;
00118     //~ position = pos;
00119     //~ lim = VPMatrix();
00120     //~ minAngle = min;
00121     //~ maxAngle = max;
00122     //~ currentPosition = cur;
00123     //~ restPosition = rest;
00124     //~ rangeModifier = rm;
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; //FixMe: Maybe a new evoluta should be created
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     // Note (by Bruno): Although not stated anywhere, it seems that when DOFs are
00156     // created, their current position is that of zero rotation
00157     currentPosition = (0-min)/(max-min);
00158     // After been set, a dof should be ready to draw
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 //~ VPMatrix
00190 //~ VPDof::GetBim() {
00191     //~ /// Build BIM.
00192     //~ axis.vpNormalize();
00193     //~ VPVector3D vx = VPVector3D(1, 0, 0);
00194     //~ VPVector3D vy = axis.vpCrossProduct(vx);
00195     //~ // Verify linearity
00196     //~ if (vy == VPVector3D(0, 0, 0)) {
00197         //~ vx = VPVector3D(0, 0, 1);
00198         //~ vy = axis.vpCrossProduct(vx);
00199     //~ }
00200     //~ vy.vpNormalize();
00201     //~ vx = vy.vpCrossProduct(axis);
00202     //~ vx.vpNormalize();
00203     //~ float           fm[4][4];
00204 
00205     //~ fm[0][0] = vx.vpGetX();
00206     //~ fm[1][0] = vx.vpGetY();
00207     //~ fm[2][0] = vx.vpGetZ();
00208     //~ fm[3][0] = 0;
00209 
00210     //~ fm[0][1] = vy.vpGetX();
00211     //~ fm[1][1] = vy.vpGetY();
00212     //~ fm[2][1] = vy.vpGetZ();
00213     //~ fm[3][1] = 0;
00214 
00215     //~ fm[0][2] = axis.vpGetX();
00216     //~ fm[1][2] = axis.vpGetY();
00217     //~ fm[2][2] = axis.vpGetZ();
00218     //~ fm[3][2] = 0;
00219 
00220     //~ fm[0][3] = 0;
00221     //~ fm[1][3] = 0;
00222     //~ fm[2][3] = 0;
00223     //~ fm[3][3] = 1;
00224 
00225     //~ VPMatrix lBim = VPMatrix(fm);
00226     //~ return lBim;
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 // FixMe: There should not exist a "SetLim" because computing it is probably the
00285 // main reason for the class to exist...
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     // Avoid values out of range
00304     if (pos > 1.0) pos = 1.0;
00305     if (pos < 0.0) pos = 0.0;
00306 
00307     // Find angles by interpolation
00308     double newAngle = currentMinAngle + pos * (currentMaxAngle - currentMinAngle);
00309     double currentAngle = currentMinAngle + currentPosition * (currentMaxAngle - currentMinAngle);
00310 
00311     // Find center of rotation
00312     VPPoint4D center = position;
00313     if (evoluta)
00314     {
00315         VPPoint4D bezierPoint;
00316         evoluta->GetPoint(currentPosition, &bezierPoint);
00317         bezierPoint.SetW(0); // Turn it into translation vector
00318         center = position + bezierPoint;
00319     }
00320 
00321     // Apply difference to other DOFs down the hierarchy
00322     VPTransform diffTrans;
00323     diffTrans.MakeRotation(center, axis, currentAngle-newAngle);
00324     ownerJoint->TransformDofs(this, diffTrans);
00325 
00326     // Update Local Instance Matrix
00327     currentPosition = pos;
00328     lim.MakeRotation(center, axis, newAngle);
00329 
00330     // Update external (joint) state
00331     ownerJoint->MakeLim();
00332 }
00333 
00334 void VPDof::ComputeLIM()
00335 {
00336     // Find angle by interpolation
00337     double angle = currentMinAngle + currentPosition * (currentMaxAngle - currentMinAngle);
00338 
00339     // Find center of rotation
00340     VPPoint4D center;
00341     if (evoluta)
00342     {
00343         VPPoint4D bezierPoint;
00344         evoluta->GetPoint(currentPosition, &bezierPoint);
00345         bezierPoint.SetW(0); // Turn it into translation vector
00346         center = position + bezierPoint;
00347     }
00348     else
00349         center = position;
00350     // Update Local Instance Matrix
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     // Try to keep floating point errors reazonable small...
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); // Turn it into translation vector
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 }

Generated on Tue Sep 6 10:00:03 2005 for VPAT by  doxygen 1.4.4