Main Page | Class Hierarchy | Alphabetical List | Class List | File List | Class Members | File Members

ArticulatedAgentBase.cpp

Go to the documentation of this file.
00001 
00002 /*
00003 
00004    How the agent is built
00005    TODO: This comment should be updated!
00006 
00007    - the agent has a circular body; radius and position are initialized, other properties 
00008    are set to 0 by default; mass and inertia are set based on default density;
00009    - two limbs are defined; for each limb is defined the angle theta and the distance l of the
00010    position of first joint, relative to the body;
00011    - we add links to the limbs:
00012       - we create the link; by default, theta0Min is -90 degrees, theta0Max is 90 degrees, 
00013       everything else is set to 0 on link creation;
00014       - we set theta0, theta as theta0, l;
00015       - we create an object and set its dimensions; mass and inertia are set based on default 
00016       density, other properties are 0;
00017       - we add the object to the link, by setting its position and angle relative to the link
00018       - we add the link to the limb; this also computes the total mass, center of mass and 
00019       inertia of the link, relative to the link point of reference; and computes IStar0; IStar 
00020       is set to IStar0.
00021       - at this moment, the absolute positions, velocities etc. of the link and objects is 0; we
00022       call forwardKinematics() to set these properties based on the absolute position of the 
00023       body, and the theta angles, thetaD's etc. of the links
00024 
00025 
00026 */
00027 
00028 #include "ArticulatedAgentBase.h"
00029 #include "Simulator.h"
00030 #include "GUI.h"
00031 #include "purgeContainer.h"
00032 
00034 // Construction/Destruction
00036 
00037     
00038 ArticulatedAgentBase::ArticulatedAgentBase(std::string label) :
00039     ArticulatedComponent(label)
00040 {
00041    nContacts=0;
00042 }
00043 
00044 
00045 ArticulatedAgentBase::~ArticulatedAgentBase(){
00046    deleteLimbs();
00047 }
00048 
00049 
00050 ArticulatedLimb* ArticulatedAgentBase::addLimb(){
00051     ArticulatedLimb* limb = new ArticulatedLimb(0, 0);
00052    limbs.push_back(limb);
00053 
00054    int i=limbs.size();
00055    char s[10];
00056    sprintf(s,"%d",i);
00057    limb->label=label+": limb "+s;
00058 
00059    return limb;
00060 }
00061 
00062 ArticulatedLimb* ArticulatedAgentBase::addLimb(real l, real theta){
00063    ArticulatedLimb* limb;
00064    limb=addLimb();
00065    limb->setProperties(l, theta);
00066    return limb;
00067 }
00068 
00069 void ArticulatedAgentBase::registerPrimitives(Simulator* simulator){
00070    //register body components
00071    ComposedPhysicalObject::registerPrimitives(simulator);
00072    for(ArticulatedLimbPVector::iterator limbsI = limbs.begin(), 
00073             limbsEnd = limbs.end(); limbsI != limbsEnd; ++limbsI){
00074         ArticulatedLimb* limb = *limbsI;
00075       for(ArticulatedLinkPVector::iterator linksI = limb->links.begin(), 
00076                 linksEnd = limb->links.end(); linksI != linksEnd; ++linksI){
00077          //register links components
00078          (*linksI)->registerPrimitives(simulator);
00079       }
00080    }
00081 }
00082 
00083 void ArticulatedAgentBase::solveSystem(SymmetricMatrix3& m, const Vector3& c, Vector3& x){
00084    real determinant;
00085    determinant=m.getDeterminant();
00086    m.mirror();
00087    solveSystem(m, determinant, c, x);
00088 }
00089 
00090 void ArticulatedAgentBase::solveSystem(const SymmetricMatrix3& m, const real determinant, 
00091                                        const Vector3& c, Vector3& x){
00092    /*
00093    m is a symmetric matrix. When we change a column, it may not remain symmetrical. mm is not symmetrical,
00094    and we should complete the lower half of m ("mirror") before assigning it to mm.
00095    */
00096    Matrix3 mm;
00097    assert(determinant!=0.0);
00098    for(int i=1; i<=3; i++){
00099       mm=m;
00100       mm.setColumn(i,c);
00101       x.setElement(i,mm.getDeterminant()/determinant);
00102    }
00103 }
00104 
00105 bool ArticulatedAgentBase::detectContacts(PhysicalObject* object, GlobalContactInfoVector* contacts){
00106    bool isContact=false;
00107    isContact = isContact || ComposedPhysicalObject::detectContacts(object, contacts);
00108    for(ArticulatedLimbPVector::iterator limbsI = limbs.begin(), 
00109             limbsEnd = limbs.end(); limbsI != limbsEnd; ++limbsI){
00110         ArticulatedLimb* limb = *limbsI;
00111       for(ArticulatedLinkPVector::iterator linksI = limb->links.begin(), 
00112                 linksEnd = limb->links.end(); linksI != linksEnd; ++linksI){
00113          isContact = (*linksI)->detectContacts(object, contacts) || isContact;
00114       }
00115    }
00116    return isContact;
00117 }
00118 
00119 void ArticulatedAgentBase::detectInternalContacts(GlobalContactInfoVector* contacts){
00120    ArticulatedLimb* limb1;
00121    ArticulatedLimb* limb2;
00122    ArticulatedLimbPVector::iterator limbsI1, limbsI2, limbsEnd;
00123    ArticulatedLinkPVector::iterator linksI1, linksEnd1, linksI2, linksEnd2;
00124    
00125    for(limbsI1=limbs.begin(), limbsEnd=limbs.end(); limbsI1!=limbsEnd; limbsI1++){
00126       limb1=*limbsI1;
00127       for (linksI1=limb1->links.begin(), linksEnd1=limb1->links.end(); linksI1!=linksEnd1; linksI1++){
00128          /* Link-body contact; we don't have contact between the body and the first link. */
00129          if(linksI1!=limb1->links.begin()) {
00130             ComposedPhysicalObject::detectContacts(*linksI1, contacts);
00131          }
00132          // Torque contact
00133          (*linksI1)->detectTorqueContact(contacts);
00134          // Link-link contact with other limbs
00135          limbsI2=limbsI1;
00136          limbsI2++;
00137          if(limbsI2!=limbsEnd){           
00138             for( ; limbsI2!=limbsEnd; limbsI2++){  
00139                limb2=*limbsI2;
00140                for (linksI2=limb2->links.begin(), linksEnd2=limb2->links.end(); linksI2!=linksEnd2; linksI2++){
00141                   (*linksI2)->detectContacts(*linksI1, contacts);
00142                }
00143             }
00144          }
00145          /* Link-link contact on the same limb; we don't have contacts between succesive links. 
00146          Assumes that the parent link appears earlier in the link list. */
00147          linksI2=linksI1;
00148          linksI2++;
00149          if(linksI2!=linksEnd1){
00150             for (; linksI2!=linksEnd1; linksI2++){
00151                if((*linksI1)!=(*linksI2)->parentLink)
00152                   (*linksI1)->detectContacts(*linksI2, contacts);
00153             }
00154          }
00155       }
00156    }
00157 }
00158 
00159 bool ArticulatedAgentBase::detectMouseContact(const Vector2& rMouse, Vector2& p, PhysicalObject*& object)   {
00160    bool isContact=false;
00161    isContact = ComposedPhysicalObject::detectMouseContact(rMouse, p, object);
00162    for(ArticulatedLimbPVector::iterator limbsI = limbs.begin(), 
00163             limbsEnd = limbs.end(); limbsI != limbsEnd && !isContact; ++limbsI){
00164         ArticulatedLimb* limb = *limbsI;
00165       for(ArticulatedLinkPVector::iterator linksI = limb->links.begin(), 
00166                 linksEnd = limb->links.end(); linksI != linksEnd && !isContact; ++linksI){
00167          isContact = (*linksI)->detectMouseContact(rMouse, p, object);
00168       }
00169    }
00170    return isContact;
00171 }
00172 
00173 void ArticulatedAgentBase::deleteContacts(){
00174    ArticulatedLimb* limb;
00175    ArticulatedLimbPVector::iterator limbsI, limbsEnd;
00176    ArticulatedLinkPVector::iterator linksI, linksEnd;
00177    ComposedPhysicalObject::deleteContacts();
00178    for(limbsI=limbs.begin(), limbsEnd=limbs.end(); limbsI!=limbsEnd; limbsI++){
00179       limb=*limbsI;
00180       for(linksI=limb->links.begin(), linksEnd=limb->links.end(); linksI!=linksEnd; linksI++){
00181          (*linksI)->deleteContacts();
00182       }
00183    }
00184 }
00185 
00186 void ArticulatedAgentBase::fillContactMatrix(ContactSolver* contactSolver, ContactInfo* contactInfo){
00187    /*
00188    contactInfo is the info of contact alpha relative to the current object
00189    Assumes v_0, w_0 (the velocity without the contribution of the contacts) are
00190    already computed as v, omega. 
00191    */
00192    Vector2 vt1, vt2;
00193    for(LinkContactInfoVector::iterator Qi=Q.begin(), Qend=Q.end(); Qi!=Qend; ++Qi){
00194       vt1.setXY(Qi->v.y,Qi->v.z);
00195       vt1.rotate(alpha);
00196       vt2=contactInfo->p;
00197       vt2.cross(Qi->v.x);
00198       vt1+=vt2;
00199       contactSolver->contactMatrix[contactInfo->alpha][Qi->alpha]+=
00200          vt1*contactInfo->n*contactInfo->sigma;    
00201    }
00202    vt1=contactInfo->p;
00203    vt1.cross(omega);
00204    vt1+=v;
00205    contactSolver->contactVector[contactInfo->alpha]+=vt1*contactInfo->n*contactInfo->sigma;
00206 }
00207 
00208 void ArticulatedAgentBase::deleteLimbs(){
00209    purgeContainer(limbs);
00210 }
00211 
00212 void ArticulatedAgentBase::draw(GUI *gui){
00213    ComposedPhysicalObject::draw(gui);
00214    for(ArticulatedLimbPVector::iterator it = limbs.begin(), end=limbs.end();
00215         it != end; ++it) {
00216             (*it)->draw(gui);
00217     }
00218 }
00219 
00220 void ArticulatedAgentBase::setOutlineColor(Color color){
00221    ComposedPhysicalObject::setOutlineColor(color);
00222    for(ArticulatedLimbPVector::iterator it = limbs.begin(), end=limbs.end();
00223         it != end; ++it) {
00224             (*it)->setOutlineColor(color);
00225     }
00226 }
00227 
00228 void ArticulatedAgentBase::setFillColor(Color color){
00229    ComposedPhysicalObject::setFillColor(color);
00230    for(ArticulatedLimbPVector::iterator it = limbs.begin(), end=limbs.end();
00231         it != end; ++it) {
00232             (*it)->setFillColor(color);
00233     }
00234 }

Thyrix homepageUsers' guide

(C) Arxia 2004-2005