cd.c
Go to the documentation of this file.
1 #include "cd.h"
2 
3 #include "defines.h"
4 #include "algebra.h"
5 #include "error.h"
6 
7 #include <math.h>
8 
57 void StoreCollisionInfoInt(FILE *f,char *fname,unsigned int objectID,
58  Tmechanism *m,unsigned int nc,TCollisionInfo *c);
59 
74  unsigned int nc,TCollisionInfo *c);
75 
76 #ifdef _HAVE_SOLID
77 
94  DT_Bool CDCallBackInfo(void *client_data,void *obj1,void *obj2,const DT_CollData *cd);
95 
108  DT_ShapeHandle AddShape2Solid(Tpolyhedron *p);
109 
123  boolean SolidCorrection(THTransform *t,Tpolyhedron *p);
124 
142  void InitSolidCD(boolean parallel,Tmechanism *m,Tenvironment *e,
143  boolean **checkCollisionsLL,boolean **checkCollisionsLO,
144  TsolidCD *s);
145 
159  boolean CheckCollisionSolid(boolean all,THTransform *tl,TLinkConf *def,TsolidCD *s);
160 
168  void DeleteSolidCD(TsolidCD *s);
169 #endif
170 
171 #ifdef _HAVE_VCOLLIDE
172 
185  boolean **checkCollisionsLL,boolean **checkCollisionsLO,
186  TvcollideCD *vc);
187 
188 
200 
208  void DeleteVcollideCD(TvcollideCD *vc);
209 
210 #endif
211 
212 #ifdef _HAVE_PQP
213 
225  void InitPQPCD(Tmechanism *m,Tenvironment *e,
226  boolean **checkCollisionsLL,boolean **checkCollisionsLO,
227  TpqpCD *p);
228 
239  boolean CheckCollisionPQP(THTransform *tl,TpqpCD *p);
240 
248  void DeletePQPCD(TpqpCD *p);
249 
250 #endif
251 
252 
253 #if _HAVE_FCL
254 
264  void DefineFCLObject(Tpolyhedron *shape,TfclCD *p);
265 
277  void InitFCLCD(Tmechanism *m,Tenvironment *e,
278  boolean **checkCollisionsLL,boolean **checkCollisionsLO,
279  TfclCD *p);
280 
293  boolean CheckCollisionFCL(boolean all,THTransform *tl,TfclCD *p);
294 
310  boolean CheckContCollisionFCL(THTransform *tl,THTransform *tlPrev,TfclCD *p);
311 
319  void DeleteFCLCD(TfclCD *p);
320 
321 #endif
322 
323 #if _HAVE_BULLET
324 
335  void DefineBulletObject(boolean fixed,Tpolyhedron *shape,TBulletCD *p);
336 
349  void InitBulletCD(Tmechanism *m,Tenvironment *e,
350  boolean **checkCollisionsLL,boolean **checkCollisionsLO,
351  boolean cont,TBulletCD *p);
352 
365  boolean CheckCollisionBullet(boolean all,THTransform *tl,TBulletCD *p);
366 
382  boolean CheckContCollisionBullet(THTransform *tl,THTransform *tlPrev,TBulletCD *p);
383 
391  void DeleteBulletCD(TBulletCD *p);
392 
393 #endif
394 
395 #ifdef _HAVE_RIGIDCLL
396 
412  boolean ObstaclesCollideLikeGround(unsigned int nl,unsigned int no,boolean **checkCollisionsLL,boolean **checkCollisionsLO);
413 
425  void InitRigidCLLCD(Tmechanism *m,Tenvironment *e,
426  boolean **checkCollisionsLL,boolean **checkCollisionsLO,
427  TrigidCLLCD *r);
428 
439  boolean CheckCollisionRigidCLL(THTransform *tl,TrigidCLLCD *r);
440 
448  void DeleteRigidCLLCD(TrigidCLLCD *r);
449 
450 #endif
451 
452 
453 #ifdef _HAVE_SOLID
454 
455  DT_Bool CDCallBackInfo(void *client_data,void *obj1,void *obj2,
456  const DT_CollData *cd)
457  {
458  TsolidCD *s;
459 
460  s=(TsolidCD *)client_data;
461 
462  if ((s->simple)||(s->collision==NULL))
463  /* in simple collision detection mode. Stop as soon as a collision is
464  detected, without collecing information about the collisions. */
465  return(DT_DONE);
466  else
467  {
468  unsigned int id1,id2,k;
469  double nr;
470 
471  if (s->nc==s->mc)
472  {
474  }
475 
476  id1=*((unsigned int*)obj1);
477  id2=*((unsigned int*)obj2);
478 
479  if (s->object[id1].linkID==NO_UINT)
480  {
481  /* the first object is a body in the environment */
482  s->collision[s->nc].isLink1=FALSE;
483  s->collision[s->nc].id1=s->object[id1].bodyID;
484  HTransformIdentity(&(s->collision[s->nc].t1));
485  }
486  else
487  {
488  s->collision[s->nc].isLink1=TRUE;
489  s->collision[s->nc].id1=s->object[id1].linkID;
490  HTransformCopy(&(s->collision[s->nc].t1),&(s->object[id1].t));
491  }
492 
493  if (s->object[id2].linkID==NO_UINT)
494  {
495  /* the second object is a body in the environment */
496  s->collision[s->nc].isLink2=FALSE;
497  s->collision[s->nc].id2=s->object[id2].bodyID;
498  HTransformIdentity(&(s->collision[s->nc].t2));
499  }
500  else
501  {
502  s->collision[s->nc].isLink2=TRUE;
503  s->collision[s->nc].id2=s->object[id2].linkID;
504  HTransformCopy(&(s->collision[s->nc].t2),&(s->object[id2].t));
505  }
506 
507  for(k=0;k<3;k++)
508  {
509  s->collision[s->nc].point[k]=cd->point2[k];
510  s->collision[s->nc].normal[k]=cd->normal[k];
511  }
512  /* Just in case ;) */
513  nr=Norm(3,s->collision[s->nc].normal);
514  if (nr>ZERO) /* In some cases is very tiny */
515  ScaleVector(1.0/nr,3,s->collision[s->nc].normal);
516 
517  s->nc++;
518 
519  /* The transforms are computed after the detection */
520  return DT_CONTINUE;
521  }
522  }
523 
524  DT_ShapeHandle AddShape2Solid(Tpolyhedron *p)
525  {
526  unsigned int i,j,k,n;
527  unsigned int nv,nf;
528  double **v;
529  unsigned int *nvf,**fv;
530  DT_ShapeHandle s=0;
531  DT_Scalar ps[3];
532  double rad;
533 
535  Error("DECOR shapes can not be included into collision detection");
536 
537  /* hidden shapes are added to the collision detection, although they
538  are not displayed */
539  /*
540  IMPORTANT: The following block can not be executed in parallel. SOLID stores all
541  the shapes in the same pool of shapes, which can not be updated
542  concurrently.
543  */
544  #pragma omp critical
545  {
546  switch(GetPolyhedronType(p))
547  {
548  case OFF:
549  case LINE:
550  GetOFFInfo(&nv,&v,&nf,&nvf,&fv,p);
551 
552  s=DT_NewComplexShape(NULL);
553  for(i=0;i<nf;i++)
554  {
555  DT_Begin();
556  for(j=0;j<nvf[i];j++)
557  {
558  n=fv[i][j];
559  for(k=0;k<3;k++)
560  ps[k]=(DT_Scalar)v[n][k];
561  DT_Vertex(ps);
562  }
563  DT_End();
564  }
565  DT_EndComplexShape();
566  break;
567  case CYLINDER:
568  /* Define a unitary cylinder along the Y axis CENTERED at the origin */
569  rad=GetPolyhedronRadius(p);
570  s=DT_NewCylinder((DT_Scalar)rad,1.0f);
571  break;
572  case SPHERE:
573  rad=GetPolyhedronRadius(p);
574  s=DT_NewSphere((DT_Scalar)rad);
575  break;
576  case SEGMENTS:
577  Error("Segment-based structures can not be included in collision detection");
578  break;
579  }
580  }
581  return(s);
582  }
583 
585  {
586  THTransform t2;
587  double c[3],p1[3],p2[3];
588 
590  Error("DECOR shapes can not be included into collision detection");
591 
592  switch(GetPolyhedronType(p))
593  {
594  case OFF:
595  case LINE:
597  break;
598  case CYLINDER:
599  /* The transforms below have to be interpreted from last to first */
600 
601  /* rotate the unitary cylinder along the X positive axis to an
602  arbitrary position (this includes scaling to the appropiate lenght
603  since we defined it with lenght 1) */
604 
607 
608  HTransformX2Vect(1,1,p1,p2,t);
609  /* rotate to align the cylinder with the X axis */
610  HTransformRz2(-1,0,&t2);
611  HTransformProduct(t,&t2,t);
612  /* move the unitary cylinder to the positive Y axis */
613  HTransformTy(0.5,&t2);
614  HTransformProduct(t,&t2,t);
615  break;
616  case SPHERE:
617  GetPolyhedronCenter(c,p);
618  HTransformTxyz(c[0],c[1],c[2],t);
619  break;
620  case SEGMENTS:
621  Error("Segment-based structures can not be included in collision detection");
622  break;
623  }
624  return(!(HTransformIsIdentity(t)));
625  }
626 
627  void InitSolidCD(boolean parallel,Tmechanism *m,Tenvironment *e,
628  boolean **checkCollisionsLL,boolean **checkCollisionsLO,
629  TsolidCD *s)
630  {
631  unsigned int i,j;
632  unsigned int lID,bID,oID,lID1,lID2;
633  unsigned int nB;
634  Tlink *l;
635  Tpolyhedron *shape;
636  double matrix[16];
637  unsigned int np,nl,no;
638 
639  nl=GetMechanismNLinks(m);
641  np=GetMechanismNBodies(m)+no;
642 
643  /* Create the shapes */
644 
645  /* np is always larger (or equal) than the number of part that can actually
646  collide. */
647  NEW(s->object,np,TsolidObj);
648 
649  /* Generate the shapes */
650  s->np=0;
651  for(lID=0;lID<nl;lID++)
652  {
653  if (LinkCanCollide(lID,nl,no,checkCollisionsLL,checkCollisionsLO))
654  {
655  l=GetMechanismLink(lID,m);
656  nB=LinkNBodies(l);
657 
658  for(bID=0;bID<nB;bID++)
659  {
660  if (GetLinkBodyStatus(bID,l)!=DECOR_SHAPE)
661  {
662  s->object[s->np].id=s->np;
663  s->object[s->np].linkID=lID;
664  s->object[s->np].bodyID=bID;
665 
666  shape=GetLinkBody(bID,l);
667  s->object[s->np].shape=AddShape2Solid(shape);
668 
669  s->object[s->np].correction=SolidCorrection(&(s->object[s->np].tc),shape);
670 
671  s->np++;
672  }
673  }
674  }
675  }
676 
677  for(oID=0;oID<no;oID++)
678  {
679  if ((ObstacleCanCollide(oID,nl,checkCollisionsLO))&&
681  {
682  s->object[s->np].id=s->np;
683  s->object[s->np].linkID=NO_UINT;
684  s->object[s->np].bodyID=oID;
685 
686  shape=GetObstacleShape(oID,e);
687  s->object[s->np].shape=AddShape2Solid(shape);
688 
689  s->object[s->np].correction=SolidCorrection(&(s->object[s->np].tc),shape);
690 
691  s->np++;
692  }
693  }
694 
695  /* Serialize the access to the internal SOLID structuers */
696  #pragma omp critical
697  {
698  s->scene=DT_CreateScene();
699  s->respTable=DT_CreateRespTable();
700 
701  for(i=0;i<s->np;i++)
702  {
703  s->object[i].handler=DT_CreateObject((void*)&(s->object[i].id),s->object[i].shape);
704  DT_AddObject(s->scene,s->object[i].handler);
705  s->object[i].respClass=DT_GenResponseClass(s->respTable);
706  DT_SetResponseClass(s->respTable,s->object[i].handler,s->object[i].respClass);
707  }
708  }
709 
710  s->simple=TRUE; /* default mode check for first collision only */
711 
712  for (i=0;i<s->np;i++)
713  {
714  lID=s->object[i].linkID;
715  if (lID!=NO_UINT)
716  {
717  /* shape 'i' is part of a link */
718  for (j=i+1;j<s->np;j++)
719  {
720  /* partj 'j' can be a link or an obstacle */
721  lID=s->object[j].linkID;
722  if (lID==NO_UINT)
723  {
724  /*link-obstacle intersection*/
725  lID=s->object[i].linkID;
726  oID=s->object[j].bodyID;
727  if (checkCollisionsLO[lID][oID])
728  {
729  DT_AddPairResponse(s->respTable,
730  s->object[i].respClass,s->object[j].respClass,
731  &CDCallBackInfo,(parallel?DT_SIMPLE_RESPONSE:DT_DEPTH_RESPONSE),s);
732  }
733  }
734  else
735  {
736  /*link-link intesection*/
737  lID1=s->object[i].linkID;
738  lID2=s->object[j].linkID;
739  if (checkCollisionsLL[lID1][lID2])
740  {
741  DT_AddPairResponse(s->respTable,
742  s->object[i].respClass,s->object[j].respClass,
743  &CDCallBackInfo,(parallel?DT_SIMPLE_RESPONSE:DT_DEPTH_RESPONSE),s);
744  }
745  }
746  }
747  }
748  }
749 
750  /* Reset the collision information (only actually used if s->simple is FALSE) */
751  s->nc=0;
752  if (parallel)
753  {
754  s->mc=0;
755  s->collision=NULL;
756  }
757  else
758  {
759  s->mc=10;
761  }
762 
763  /* For the obstacle the pre-computed correction is used only once
764  (they do not move) and we do it now*/
765  for(i=0;i<s->np;i++)
766  {
767  lID=s->object[i].linkID;
768  if ((lID==NO_UINT)&&(s->object[i].correction))
769  {
770  HTransform2GLMatrix(matrix,&(s->object[i].tc));
771  DT_SetMatrixd(s->object[i].handler,matrix);
772  }
773  /* Set a default value for all parts (mobile or not) */
774  HTransformIdentity(&(s->object[i].t));
775  }
776  }
777 
778  boolean CheckCollisionSolid(boolean all,THTransform *tl,TLinkConf *def,TsolidCD *s)
779  {
780  unsigned int i;
781  double m[16];
782  THTransform t,*to;
783  unsigned int lID;
784 
785  /* This is the only function where we actually move
786  checking for collisions. This is the place where we
787  reset the collision information before checking. */
788 
789  for(i=0;i<s->np;i++)
790  {
791  lID=s->object[i].linkID;
792  if (lID!=NO_UINT)
793  {
794  /*this part can move -> move it !*/
795 
796  /* Keep the transform before the inernal_conf/correction. */
797  to=&(s->object[i].t);
798  HTransformCopy(to,&(tl[lID]));
799 
800  /* The transform for the current link */
801  HTransformCopy(&t,to);
802 
803  /* if necessary, apply an object internal configuration */
804  if (!EmptyLinkConf(&(def[lID])))
805  {
806  THTransform d;
807 
808  GetLinkConfTransform(s->object[i].bodyID,&d,&(def[lID]));
809  HTransformProduct(&t,&d,&t);
810  HTransformDelete(&d);
811  }
812 
813  /* if necessary apply the correction */
814  if (s->object[i].correction)
815  HTransformProduct(&t,&(s->object[i].tc),&t);
816 
817  HTransform2GLMatrix(m,&t);
818  HTransformDelete(&t);
819 
820  /* Here send the transform to SOLID */
821  DT_SetMatrixd(s->object[i].handler,m);
822  }
823  }
824 
825  /* set the work mode */
826  s->simple=((!all)||(s->collision==NULL));
827 
828  /* Reset the number of collisions detected so far (only used if all=TRUE). */
829  s->nc=0;
830 
831  /* Test */
832  return(DT_Test(s->scene,s->respTable)>0);
833  }
834 
836  {
837  #pragma omp critical
838  {
839  unsigned int i;
840 
841  for(i=0;i<s->np;i++)
842  {
843  DT_RemoveObject(s->scene,s->object[i].handler);
844  DT_DestroyObject(s->object[i].handler);
845  DT_DeleteShape(s->object[i].shape);
846  HTransformDelete(&(s->object[i].t));
847  HTransformDelete(&(s->object[i].tc));
848  }
849 
850  DT_DestroyRespTable(s->respTable);
851  DT_DestroyScene(s->scene);
852  }
853  free(s->object);
854  if (s->collision!=NULL)
855  free(s->collision);
856  }
857 #endif
858 
859 #ifdef _HAVE_VCOLLIDE
860 
862  boolean **checkCollisionsLL,boolean **checkCollisionsLO,
863  TvcollideCD *vc)
864  {
865  unsigned int i,j,lID1,lID2,bID2,np,nl,no,nB,lID,bID,oID;
866  double **v;
867  unsigned int nv,nf,*nvf,**fv;
868  Tpolyhedron *shape;
869  Tlink *l;
870 
871  vc->vc=InitVcollide();
872 
873  nl=GetMechanismNLinks(m);
875  np=GetMechanismNBodies(m)+no;
876 
877  /* Create the shapes */
878 
879  /* np is always larger (or equal) than the number of part that can actually
880  collide. */
881  NEW(vc->bodyID,np,unsigned int);
882  NEW(vc->linkID,np,unsigned int);
883  NEW(vc->vcID,np,unsigned int);
884 
885  /* Generate the shapes */
886  vc->np=0;
887  for(lID=0;lID<nl;lID++)
888  {
889  if (LinkCanCollide(lID,nl,no,checkCollisionsLL,checkCollisionsLO))
890  {
891  l=GetMechanismLink(lID,m);
892  nB=LinkNBodies(l);
893 
894  for(bID=0;bID<nB;bID++)
895  {
896  if (GetLinkBodyStatus(bID,l)!=DECOR_SHAPE)
897  {
898  vc->linkID[vc->np]=lID;
899  vc->bodyID[vc->np]=bID;
900 
901  shape=GetLinkBody(bID,l);
902  GetOFFInfo(&nv,&v,&nf,&nvf,&fv,shape);
903  vc->vcID[vc->np]=AddPolyhedron2Vcollide(v,nf,nvf,fv,vc->vc);
904 
905  vc->np++;
906  }
907  }
908  }
909  }
910 
911  for(oID=0;oID<no;oID++)
912  {
913  if ((ObstacleCanCollide(oID,nl,checkCollisionsLO))&&
915  {
916  vc->linkID[vc->np]=NO_UINT;
917  vc->bodyID[vc->np]=oID;
918 
919  shape=GetObstacleShape(oID,e);
920  GetOFFInfo(&nv,&v,&nf,&nvf,&fv,shape);
921  vc->vcID[vc->np]=AddPolyhedron2Vcollide(v,nf,nvf,fv,vc->vc);
922 
923  vc->np++;
924  }
925  }
926 
927  /* Now define the possible collisions */
928  for(i=0;i<vc->np;i++)
929  {
930  lID1=vc->linkID[i];
931  if (lID1!=NO_UINT) /* if link1 is an obstacle, skip */
932  {
933  for(j=i+1;j<vc->np;j++)
934  {
935  lID2=vc->linkID[j];
936  if (lID2!=NO_UINT)
937  {
938  /* link-link */
939  if (checkCollisionsLL[lID1][lID2])
940  ActivateCollisionsVcollide(vc->vcID[i],vc->vcID[j],vc->vc);
941  else
942  DeactivateCollisionsVcollide(vc->vcID[i],vc->vcID[j],vc->vc);
943  }
944  else
945  {
946  /* link-obstacle */
947  bID2=vc->bodyID[j];
948  if (checkCollisionsLO[lID1][bID2])
949  ActivateCollisionsVcollide(vc->vcID[i],vc->vcID[j],vc->vc);
950  else
951  DeactivateCollisionsVcollide(vc->vcID[i],vc->vcID[j],vc->vc);
952  }
953  }
954  }
955  }
956  }
957 
959  {
960  unsigned int i,lID;
961 
962  /* move */
963  for(i=0;i<vc->np;i++)
964  {
965  lID=vc->linkID[i];
966  if (lID!=NO_UINT) /* if link is an obstacle, do not move */
967  MoveVcollideObject(vc->vcID[i],&(tl[lID]),vc->vc);
968  }
969 
970  /* and check */
971  return(VcollideTest(vc->vc));
972  }
973 
975  {
976  unsigned int i;
977 
978  for(i=0;i<vc->np;i++)
979  DeleteVcollideObject(vc->vcID[i],vc->vc);
980 
981  DeleteVcollide(vc->vc);
982 
983  free(vc->vcID);
984  free(vc->linkID);
985  free(vc->bodyID);
986  }
987 
988 #endif
989 
990 #ifdef _HAVE_PQP
991 
993  boolean **checkCollisionsLL,boolean **checkCollisionsLO,
994  TpqpCD *p)
995  {
996  unsigned int i,j,lID1,lID2,bID2,np,nl,no,nB,lID,bID,oID;
997  double **v;
998  unsigned int nv,nf,*nvf,**fv;
999  Tpolyhedron *shape;
1000  Tlink *l;
1001 
1002  nl=GetMechanismNLinks(m);
1004  np=GetMechanismNBodies(m)+no;
1005 
1006  /* np is always larger (or equal) than the number of part that can actually
1007  collide. */
1008  NEW(p->bodyID,np,unsigned int);
1009  NEW(p->linkID,np,unsigned int);
1010  NEW(p->model,np,Tpqp *);
1011 
1012  /* Generate the shapes */
1013  p->np=0;
1014  for(lID=0;lID<nl;lID++)
1015  {
1016  if (LinkCanCollide(lID,nl,no,checkCollisionsLL,checkCollisionsLO))
1017  {
1018  l=GetMechanismLink(lID,m);
1019  nB=LinkNBodies(l);
1020 
1021  for(bID=0;bID<nB;bID++)
1022  {
1023  if (GetLinkBodyStatus(bID,l)!=DECOR_SHAPE)
1024  {
1025  p->linkID[p->np]=lID;
1026  p->bodyID[p->np]=bID;
1027 
1028  shape=GetLinkBody(bID,l);
1029  GetOFFInfo(&nv,&v,&nf,&nvf,&fv,shape);
1030  p->model[p->np]=DefinePQPModel(v,nf,nvf,fv);
1031 
1032  p->np++;
1033  }
1034  }
1035  }
1036  }
1037 
1038  for(oID=0;oID<no;oID++)
1039  {
1040  if ((ObstacleCanCollide(oID,nl,checkCollisionsLO))&&
1042  {
1043  p->linkID[p->np]=NO_UINT;
1044  p->bodyID[p->np]=oID;
1045 
1046  shape=GetObstacleShape(oID,e);
1047  GetOFFInfo(&nv,&v,&nf,&nvf,&fv,shape);
1048  p->model[p->np]=DefinePQPModel(v,nf,nvf,fv);
1049 
1050  p->np++;
1051  }
1052  }
1053 
1054  /* Now define the possible collisions */
1055 
1056  NEW(p->p1,p->np*(p->np-1),unsigned int);
1057  NEW(p->p2,p->np*(p->np-1),unsigned int);
1058  p->nPairs=0;
1059  for(i=0;i<p->np;i++)
1060  {
1061  lID1=p->linkID[i];
1062  if (lID1!=NO_UINT) /* if link1 is an obstacle, skip */
1063  {
1064  for(j=i+1;j<p->np;j++)
1065  {
1066  lID2=p->linkID[j];
1067  if (lID2!=NO_UINT)
1068  {
1069  /* link-link */
1070  if (checkCollisionsLL[lID1][lID2])
1071  {
1072  p->p1[p->nPairs]=i;
1073  p->p2[p->nPairs]=j;
1074  p->nPairs++;
1075  }
1076  }
1077  else
1078  {
1079  /* link-obstacle */
1080  bID2=p->bodyID[j];
1081  if (checkCollisionsLO[lID1][bID2])
1082  {
1083  p->p1[p->nPairs]=i;
1084  p->p2[p->nPairs]=j;
1085  p->nPairs++;
1086  }
1087  }
1088  }
1089  }
1090  }
1091  }
1092 
1094  {
1095  boolean collision;
1096  unsigned int n,i,j,l1,l2;
1097  THTransform *t1,*t2;
1098 
1099  n=0;
1100  collision=FALSE;
1101  while((!collision)&&(n<p->nPairs))
1102  {
1103  i=p->p1[n];
1104  j=p->p2[n];
1105 
1106  l1=p->linkID[i];
1107  l2=p->linkID[j];
1108 
1109  if (l1!=NO_UINT)
1110  t1=&(tl[l1]);
1111  else
1112  t1=NULL;
1113 
1114  if (l2!=NO_UINT)
1115  t2=&(tl[l2]);
1116  else
1117  t2=NULL;
1118 
1119  collision=PQPTest(t1,p->model[i],t2,p->model[j]);
1120 
1121  n++;
1122  }
1123  return(collision);
1124  }
1125 
1127  {
1128  unsigned int i;
1129 
1130  for(i=0;i<p->np;i++)
1131  DeletePQPModel(p->model[i]);
1132 
1133  free(p->model);
1134  free(p->linkID);
1135  free(p->bodyID);
1136  free(p->p1);
1137  free(p->p2);
1138  }
1139 
1140 #endif
1141 
1142 #if _HAVE_FCL
1143 
1144  void DefineFCLObject(Tpolyhedron *shape,TfclCD *p)
1145  {
1146  double p1[3],p2[3],r,l;
1147  double **v;
1148  unsigned int nv,nf,*nvf,**fv;
1149  THTransform t1,t2;
1150  TFCLObject *obj;
1151 
1152  obj=&(p->object[p->np]);
1153 
1154  switch(GetPolyhedronType(shape))
1155  {
1156  case SPHERE:
1157  r=GetPolyhedronRadius(shape);
1158  obj->model=DefineFCLSphere(r);
1159  GetPolyhedronCenter(p1,shape);
1160  HTransformTxyz(p1[0],p1[1],p1[2],&(obj->tc));
1161 
1162  /* Set the type */
1163  obj->type=FCL_SPHERE;
1164  break;
1165 
1166  case CYLINDER:
1167  /* vector with unit length along the Z axis (zero centered) */
1168  r=GetPolyhedronRadius(shape);
1169  GetPolyhedronDefiningPoint(0,p1,shape);
1170  GetPolyhedronDefiningPoint(1,p2,shape);
1171  SubtractVector(3,p2,p1); /* p2=p2-p1 */
1172  l=Norm(3,p2);
1173  obj->model=DefineFCLCylinder(r,l);
1174 
1175  /* define the correction: apply from end to top */
1176  /* the transforms must be proper rigid transforms (do not use
1177  scale/deformation) */
1178  /* Move the x-aligned cylinder to an arbitrary position (3) */
1179  SumVectorScale(3,p1,1.0/l,p2,p2);
1180  HTransformX2Vect(1,1,p1,p2,&t1);
1181  /* rotate to align the cylinder with the X axis (2) */
1182  HTransformRy2(1,0,&t2);
1183  HTransformProduct(&t1,&t2,&t1);
1184  /* move the cylinder to the positive Z axis (1) */
1185  HTransformTz(0.5*l,&t2);
1186  HTransformProduct(&t1,&t2,&(obj->tc));
1187 
1188  /* Set the type */
1189  obj->type=FCL_CYLINDER;
1190  break;
1191 
1192  default:
1193  /* The soup of triangles is defined and works for any body */
1194  GetOFFInfo(&nv,&v,&nf,&nvf,&fv,shape);
1195  obj->model=DefineFCLMesh(nv,v,nf,nvf,fv);
1196  HTransformIdentity(&(obj->tc));
1197 
1198  /* Set the type */
1199  obj->type=FCL_MESH;
1200  }
1201  }
1202 
1203  void InitFCLCD(Tmechanism *m,Tenvironment *e,
1204  boolean **checkCollisionsLL,boolean **checkCollisionsLO,
1205  TfclCD *p)
1206  {
1207  unsigned int i,j,k,lID1,lID2,bID2,np,nl,no,nB,lID,bID,oID;
1208  Tpolyhedron *shape;
1209  Tlink *l;
1210 
1211  nl=GetMechanismNLinks(m);
1213  np=GetMechanismNBodies(m)+no;
1214 
1215  /* np is always larger (or equal) than the number of part that can actually
1216  collide. */
1217  NEW(p->object,np,TFCLObject);
1218 
1219  /* Generate the shapes */
1220  p->np=0;
1221  for(lID=0;lID<nl;lID++)
1222  {
1223  if (LinkCanCollide(lID,nl,no,checkCollisionsLL,checkCollisionsLO))
1224  {
1225  l=GetMechanismLink(lID,m);
1226  nB=LinkNBodies(l);
1227 
1228  for(bID=0;bID<nB;bID++)
1229  {
1230  if (GetLinkBodyStatus(bID,l)!=DECOR_SHAPE)
1231  {
1232  p->object[p->np].linkID=lID;
1233  p->object[p->np].bodyID=bID;
1234 
1235  shape=GetLinkBody(bID,l);
1236  DefineFCLObject(shape,p);
1237  p->np++;
1238  }
1239  }
1240  }
1241  }
1242  k=p->np; /* mobile parts */
1243  for(oID=0;oID<no;oID++)
1244  {
1245  if ((ObstacleCanCollide(oID,nl,checkCollisionsLO))&&
1247  {
1248  p->object[p->np].linkID=NO_UINT;
1249  p->object[p->np].bodyID=oID;
1250 
1251  shape=GetObstacleShape(oID,e);
1252  DefineFCLObject(shape,p);
1253  p->np++;
1254  }
1255  }
1256 
1257  /* Now define the possible collisions */
1258  k=k*(p->np-1); /* k=k*(k-1)+k*(p->np-k) link-link link-obs */
1259  NEW(p->p1,k,unsigned int);
1260  NEW(p->p2,k,unsigned int);
1261 
1262  p->nPairs=0;
1263  for(i=0;i<p->np;i++)
1264  {
1265  lID1=p->object[i].linkID;
1266  if (lID1!=NO_UINT) /* if object is an obstacle, skip */
1267  {
1268  for(j=i+1;j<p->np;j++)
1269  {
1270  lID2=p->object[j].linkID;
1271  if (lID2!=NO_UINT)
1272  {
1273  /* link-link */
1274  if (checkCollisionsLL[lID1][lID2])
1275  {
1276  p->p1[p->nPairs]=i;
1277  p->p2[p->nPairs]=j;
1278  p->nPairs++;
1279  }
1280  }
1281  else
1282  {
1283  /* link-obstacle */
1284  bID2=p->object[j].bodyID;
1285  if (checkCollisionsLO[lID1][bID2])
1286  {
1287  p->p1[p->nPairs]=i;
1288  p->p2[p->nPairs]=j;
1289  p->nPairs++;
1290  }
1291  }
1292  }
1293  }
1294  }
1295 
1296  p->nc=0;
1297  p->mc=10;
1298  NEW(p->collision,p->mc,TCollisionInfo);
1299 
1300  /* set the default transform for each part */
1301  for(i=0;i<p->np;i++)
1302  HTransformCopy(&(p->object[i].t),&(p->object[i].tc));
1303  }
1304 
1305  boolean CheckCollisionFCL(boolean all,THTransform *tl,TfclCD *p)
1306  {
1307  boolean collision,done,newCollision;
1308  unsigned int i,j,lID;
1309  signed int n;
1310  double point[3],normal[3];
1311  TFCLObject *obj1,*obj2;
1312 
1313  /* Precompute the transforms for each part */
1314  for(i=0;i<p->np;i++)
1315  {
1316  lID=p->object[i].linkID;
1317  /* the transforms for static objecs are already pre-computed */
1318  if (lID!=NO_UINT)
1319  {
1320  if (p->object[i].type==FCL_MESH) /* meshes have no offset */
1321  HTransformCopy(&(p->object[i].t),&(tl[lID]));
1322  else
1323  HTransformProduct(&(tl[lID]),&(p->object[i].tc),&(p->object[i].t));
1324  }
1325  }
1326 
1327  p->nc=0; /* Reset the collision information. Will be updated probably. */
1328  n=0;
1329  collision=FALSE;
1330  done=FALSE;
1331  while((!done)&&(n<p->nPairs))
1332  {
1333  i=p->p1[n];
1334  j=p->p2[n];
1335  obj1=&(p->object[i]);
1336  obj2=&(p->object[j]);
1337 
1338  newCollision=FCLTest(all,
1339  obj1->model,&(obj1->t),
1340  obj2->model,&(obj2->t),
1341  point,normal);
1342 
1343  if ((all)&&(newCollision))
1344  {
1345  unsigned int l1,l2,k;
1346  TCollisionInfo *col;
1347 
1348  /* collect the collision results */
1349  if (p->nc==p->mc)
1350  {
1351  MEM_DUP(p->collision,p->mc,TCollisionInfo);
1352  }
1353 
1354  col=&(p->collision[p->nc]);
1355 
1356  l1=obj1->linkID;
1357  if (l1==NO_UINT)
1358  {
1359  col->isLink1=FALSE;
1360  col->id1=obj1->bodyID;
1361  }
1362  else
1363  {
1364  col->isLink1=TRUE;
1365  col->id1=l1;
1366  }
1367 
1368  l2=obj2->linkID;
1369  if (l2==NO_UINT)
1370  {
1371  col->isLink2=FALSE;
1372  col->id2=obj2->bodyID;
1373  }
1374  else
1375  {
1376  col->isLink2=TRUE;
1377  col->id2=l2;
1378  }
1379 
1380  for(k=0;k<3;k++)
1381  {
1382  col->point[k]=point[k];
1383  col->normal[k]=normal[k];
1384  }
1385  /* Just in case ;) */
1386  Normalize(3,col->normal);
1387 
1388  /* Store the transforms as given by the user */
1389  if (col->isLink1)
1390  HTransformCopy(&(col->t1),&(tl[col->id1]));
1391  else
1392  HTransformIdentity(&(col->t1));
1393 
1394  if (col->isLink2)
1395  HTransformCopy(&(col->t2),&(tl[col->id2]));
1396  else
1397  HTransformIdentity(&(col->t2));
1398 
1399  p->nc++;
1400  }
1401 
1402  collision=((collision)||(newCollision));
1403 
1404  done=((!all)&&(newCollision));
1405 
1406  n++;
1407  }
1408  return(collision);
1409  }
1410 
1411  boolean CheckContCollisionFCL(THTransform *tl,THTransform *tlPrev,TfclCD *p)
1412  {
1413  if (tlPrev==NULL)
1414  return(CheckCollisionFCL(FALSE,tl,p));
1415  else
1416  {
1417  boolean collision;
1418  unsigned int n,i,j,lID;
1419  THTransform *tp;
1420  TFCLObject *obj1,*obj2;
1421 
1422  /* Precompute the transform */
1423  NEW(tp,p->np,THTransform);
1424 
1425  /* Precompute the transforms for each part */
1426  for(i=0;i<p->np;i++)
1427  {
1428  obj1=&(p->object[i]);
1429 
1430  lID=obj1->linkID;
1431 
1432  if (lID!=NO_UINT)
1433  {
1434  if (obj1->type==FCL_MESH)
1435  {
1436  HTransformCopy(&(obj1->t),&(tl[lID]));
1437  HTransformCopy(&(tp[i]),&(tlPrev[lID]));
1438  }
1439  else
1440  {
1441  HTransformProduct(&(tl[lID]),&(obj1->tc),&(obj1->t));
1442  HTransformProduct(&(tlPrev[lID]),&(obj1->tc),&(tp[i]));
1443  }
1444  }
1445  else
1446  HTransformCopy(&(tp[i]),&(obj1->tc)); /* static object -> copy the offset, if any */
1447  }
1448 
1449 
1450  p->nc=0; /* Reset the collision information. Not updated here. */
1451  n=0;
1452  collision=FALSE;
1453  while((!collision)&&(n<p->nPairs))
1454  {
1455  i=p->p1[n];
1456  j=p->p2[n];
1457  obj1=&(p->object[i]);
1458  obj2=&(p->object[j]);
1459 
1460  collision=FCLCTest(obj1->model,&(tp[i]),&(obj1->t),
1461  obj2->model,&(tp[j]),&(obj2->t));
1462 
1463  n++;
1464  }
1465 
1466  /* release the allocated memory */
1467  free(tp);
1468 
1469  return(collision);
1470  }
1471  }
1472 
1473  void DeleteFCLCD(TfclCD *p)
1474  {
1475  unsigned int i;
1476 
1477  for(i=0;i<p->np;i++)
1478  DeleteFCLObject(p->object[i].model);
1479 
1480  free(p->object);
1481  free(p->p1);
1482  free(p->p2);
1483  free(p->collision);
1484  }
1485 
1486 #endif
1487 
1488 #ifdef _HAVE_BULLET
1489 
1490  void DefineBulletObject(boolean fixed,Tpolyhedron *shape,TBulletCD *p)
1491  {
1492  double p1[3],p2[3],r,l;
1493  double **v;
1494  unsigned int nv,nf,*nvf,**fv;
1495  THTransform t1,t2;
1496  TBulletObject *obj;
1497 
1498  obj=&(p->object[p->np]);
1499 
1500  switch(GetPolyhedronType(shape))
1501  {
1502  case SPHERE:
1503  r=GetPolyhedronRadius(shape);
1504  obj->model=DefineBulletSphere(fixed,r);
1505  GetPolyhedronCenter(p1,shape);
1506  HTransformTxyz(p1[0],p1[1],p1[2],&(obj->tc));
1507 
1508  /* Set the type */
1509  obj->type=BULLET_SPHERE;
1510  break;
1511 
1512  case CYLINDER:
1513  /* vector with unit length along the X axis (zero centered) */
1514  r=GetPolyhedronRadius(shape);
1515  GetPolyhedronDefiningPoint(0,p1,shape);
1516  GetPolyhedronDefiningPoint(1,p2,shape);
1517  SubtractVector(3,p2,p1); /* p2=p2-p1 */
1518  l=Norm(3,p2);
1519  obj->model=DefineBulletCylinder(fixed,r,l);
1520 
1521  /* define the correction: apply from end to top */
1522  /* the transforms must be proper rigid transforms (do not use
1523  scale/deformation) */
1524  /* Move the x-aligned cylinder to an arbitrary position (3) */
1525  SumVectorScale(3,p1,1.0/l,p2,p2);
1526  HTransformX2Vect(1,1,p1,p2,&t1);
1527 
1528  /* move the cylinder to the positive X axis (1) */
1529  HTransformTx(0.5*l,&t2);
1530  HTransformProduct(&t1,&t2,&(obj->tc));
1531 
1532  /* Set the type */
1533  obj->type=BULLET_CYLINDER;
1534  break;
1535 
1536  default:
1537  /* The soup of triangles is defined and works for any body */
1538  GetOFFInfo(&nv,&v,&nf,&nvf,&fv,shape);
1539  obj->model=DefineBulletMesh(fixed,nv,v,nf,nvf,fv);
1540  HTransformIdentity(&(obj->tc));
1541 
1542  /* Set the type */
1543  obj->type=BULLET_MESH;
1544  }
1545  }
1546 
1547  void InitBulletCD(Tmechanism *m,Tenvironment *e,
1548  boolean **checkCollisionsLL,boolean **checkCollisionsLO,
1549  boolean cont,TBulletCD *p)
1550  {
1551  unsigned int i,j,k,lID1,lID2,bID2,np,nl,no,nB,lID,bID,oID;
1552  Tpolyhedron *shape;
1553  Tlink *l;
1554 
1555  nl=GetMechanismNLinks(m);
1557  np=GetMechanismNBodies(m)+no;
1558 
1559  /* np is always larger (or equal) than the number of part that can actually
1560  collide. */
1561  NEW(p->object,np,TBulletObject);
1562 
1563  /* Generate the shapes */
1564  p->np=0;
1565  for(lID=0;lID<nl;lID++)
1566  {
1567  if (LinkCanCollide(lID,nl,no,checkCollisionsLL,checkCollisionsLO))
1568  {
1569  l=GetMechanismLink(lID,m);
1570  nB=LinkNBodies(l);
1571 
1572  for(bID=0;bID<nB;bID++)
1573  {
1574  if (GetLinkBodyStatus(bID,l)!=DECOR_SHAPE)
1575  {
1576  p->object[p->np].linkID=lID;
1577  p->object[p->np].bodyID=bID;
1578 
1579  shape=GetLinkBody(bID,l);
1580  DefineBulletObject(IsGroundLink(lID),shape,p);
1581  p->np++;
1582  }
1583  }
1584  }
1585  }
1586  k=p->np; /* mobile parts */
1587  for(oID=0;oID<no;oID++)
1588  {
1589  if ((ObstacleCanCollide(oID,nl,checkCollisionsLO))&&
1591  {
1592  p->object[p->np].linkID=NO_UINT;
1593  p->object[p->np].bodyID=oID;
1594 
1595  shape=GetObstacleShape(oID,e);
1596  DefineBulletObject(TRUE,shape,p);
1597  p->np++;
1598  }
1599  }
1600 
1601  /* Now define the possible collisions */
1602  k=k*(p->np-1); /* k=k*(k-1)+k*(p->np-k) link-link link-obs */
1603  NEW(p->p1,k,unsigned int);
1604  NEW(p->p2,k,unsigned int);
1605 
1606  p->nPairs=0;
1607  for(i=0;i<p->np;i++)
1608  {
1609  lID1=p->object[i].linkID;
1610  if (lID1!=NO_UINT) /* if object is an obstacle, skip */
1611  {
1612  for(j=i+1;j<p->np;j++)
1613  {
1614  lID2=p->object[j].linkID;
1615  if (lID2!=NO_UINT)
1616  {
1617  /* link-link */
1618  if (checkCollisionsLL[lID1][lID2])
1619  {
1620  p->p1[p->nPairs]=i;
1621  p->p2[p->nPairs]=j;
1622  p->nPairs++;
1623  }
1624  }
1625  else
1626  {
1627  /* link-obstacle */
1628  bID2=p->object[j].bodyID;
1629  if (checkCollisionsLO[lID1][bID2])
1630  {
1631  p->p1[p->nPairs]=i;
1632  p->p2[p->nPairs]=j;
1633  p->nPairs++;
1634  }
1635  }
1636  }
1637  }
1638  }
1639 
1640  p->nc=0;
1641  p->mc=10;
1642  NEW(p->collision,p->mc,TCollisionInfo);
1643 
1644  /* set the default transform for each part */
1645  for(i=0;i<p->np;i++)
1646  HTransformCopy(&(p->object[i].t),&(p->object[i].tc));
1647 
1648  /* Initialize the bullet world structure */
1649  p->bulletWorld=InitBulletWorld(cont);
1650  }
1651 
1652  boolean CheckCollisionBullet(boolean all,THTransform *tl,TBulletCD *p)
1653  {
1654  boolean collision,done,newCollision;
1655  unsigned int i,j,lID;
1656  signed int n;
1657  double point[3],normal[3];
1658  TBulletObject *obj1,*obj2;
1659 
1660  /* Precompute the transforms for each part */
1661  for(i=0;i<p->np;i++)
1662  {
1663  lID=p->object[i].linkID;
1664  /* the transforms for static objecs are already pre-computed */
1665  if (lID!=NO_UINT)
1666  {
1667  if (p->object[i].type==BULLET_MESH) /* meshes have no offset */
1668  HTransformCopy(&(p->object[i].t),&(tl[lID]));
1669  else
1670  HTransformProduct(&(tl[lID]),&(p->object[i].tc),&(p->object[i].t));
1671  }
1672  }
1673 
1674  p->nc=0; /* Reset the collision information. Will be updated probably. */
1675  n=0;
1676  collision=FALSE;
1677  done=FALSE;
1678  while((!done)&&(n<p->nPairs))
1679  {
1680  i=p->p1[n];
1681  j=p->p2[n];
1682  obj1=&(p->object[i]);
1683  obj2=&(p->object[j]);
1684 
1685  newCollision=BulletTest(all,
1686  obj1->model,&(obj1->t),
1687  obj2->model,&(obj2->t),
1688  point,normal,p->bulletWorld);
1689 
1690  if ((all)&&(newCollision))
1691  {
1692  unsigned int l1,l2,k;
1693  TCollisionInfo *col;
1694 
1695  /* collect the collision results */
1696  if (p->nc==p->mc)
1697  {
1698  MEM_DUP(p->collision,p->mc,TCollisionInfo);
1699  }
1700 
1701  col=&(p->collision[p->nc]);
1702 
1703  l1=obj1->linkID;
1704  if (l1==NO_UINT)
1705  {
1706  col->isLink1=FALSE;
1707  col->id1=obj1->bodyID;
1708  }
1709  else
1710  {
1711  col->isLink1=TRUE;
1712  col->id1=l1;
1713  }
1714 
1715  l2=obj2->linkID;
1716  if (l2==NO_UINT)
1717  {
1718  col->isLink2=FALSE;
1719  col->id2=obj2->bodyID;
1720  }
1721  else
1722  {
1723  col->isLink2=TRUE;
1724  col->id2=l2;
1725  }
1726 
1727  for(k=0;k<3;k++)
1728  {
1729  col->point[k]=point[k];
1730  col->normal[k]=normal[k];
1731  }
1732  /* Just in case ;) */
1733  Normalize(3,col->normal);
1734 
1735  /* Store the transforms as given by the user */
1736  if (col->isLink1)
1737  HTransformCopy(&(col->t1),&(tl[col->id1]));
1738  else
1739  HTransformIdentity(&(col->t1));
1740 
1741  if (col->isLink2)
1742  HTransformCopy(&(col->t2),&(tl[col->id2]));
1743  else
1744  HTransformIdentity(&(col->t2));
1745 
1746  p->nc++;
1747  }
1748 
1749  collision=((collision)||(newCollision));
1750 
1751  done=((!all)&&(newCollision));
1752 
1753  n++;
1754  }
1755  return(collision);
1756  }
1757 
1758  boolean CheckContCollisionBullet(THTransform *tl,THTransform *tlPrev,TBulletCD *p)
1759  {
1760  if (tlPrev==NULL)
1761  return(CheckCollisionBullet(FALSE,tl,p));
1762  else
1763  {
1764  boolean collision;
1765  unsigned int n,i,j,lID;
1766  THTransform *tp;
1767  TBulletObject *obj1,*obj2;
1768 
1769  /* Precompute the transform */
1770  NEW(tp,p->np,THTransform);
1771 
1772  /* Precompute the transforms for each part */
1773  for(i=0;i<p->np;i++)
1774  {
1775  obj1=&(p->object[i]);
1776 
1777  lID=obj1->linkID;
1778 
1779  if (lID!=NO_UINT)
1780  {
1781  if (obj1->type==BULLET_MESH)
1782  {
1783  HTransformCopy(&(obj1->t),&(tl[lID]));
1784  HTransformCopy(&(tp[i]),&(tlPrev[lID]));
1785  }
1786  else
1787  {
1788  HTransformProduct(&(tl[lID]),&(obj1->tc),&(obj1->t));
1789  HTransformProduct(&(tlPrev[lID]),&(obj1->tc),&(tp[i]));
1790  }
1791  }
1792  else
1793  HTransformCopy(&(tp[i]),&(obj1->tc)); /* static object -> copy the offset, if any */
1794  }
1795 
1796 
1797  p->nc=0; /* Reset the collision information. Not updated here. */
1798  n=0;
1799  collision=FALSE;
1800  while((!collision)&&(n<p->nPairs))
1801  {
1802  i=p->p1[n];
1803  j=p->p2[n];
1804  obj1=&(p->object[i]);
1805  obj2=&(p->object[j]);
1806 
1807  collision=BulletCTest(obj1->model,&(tp[i]),&(obj1->t),
1808  obj2->model,&(tp[j]),&(obj2->t),p->bulletWorld);
1809 
1810  n++;
1811  }
1812 
1813  /* release the allocated memory */
1814  free(tp);
1815 
1816  return(collision);
1817  }
1818  }
1819 
1820  void DeleteBulletCD(TBulletCD *p)
1821  {
1822  unsigned int i;
1823 
1824  for(i=0;i<p->np;i++)
1825  DeleteBulletObject(p->object[i].model);
1826 
1827  free(p->object);
1828  free(p->p1);
1829  free(p->p2);
1830  free(p->collision);
1831 
1832  DeleteBulletWorld(p->bulletWorld);
1833  }
1834 #endif
1835 
1836 #ifdef _HAVE_RIGIDCLL
1837 
1838  boolean ObstaclesCollideLikeGround(unsigned int nl,unsigned int no,boolean **checkCollisionsLL,boolean **checkCollisionsLO)
1839  {
1840  unsigned int i,j;
1841  boolean same;
1842 
1843  same=TRUE;
1844  for(i=0;((same)&&(i<no));i++)
1845  {
1846  for(j=0;((same)&&(j<nl));j++)
1847  same=(checkCollisionsLO[j][i]==checkCollisionsLL[j][0]);
1848  }
1849  return(same);
1850  }
1851 
1852 
1853  void InitRigidCLLCD(Tmechanism *m,Tenvironment *e,
1854  boolean **checkCollisionsLL,boolean **checkCollisionsLO,
1855  TrigidCLLCD *r)
1856  {
1857  unsigned int lID1,lID2,np,no,npl,nB,lID,bID,oID,atomID;
1858  double *center,rad;
1859  Tpolyhedron *shape;
1860  Tlink *l;
1861  boolean obsLikeGround,added;
1862 
1863  r->r=InitRigidCLL();
1864 
1865  r->nl=GetMechanismNLinks(m);
1867 
1868  /* Create the shapes */
1869  NEW(center,3,double);
1870  NEW(r->objID,r->nl+no,unsigned int);
1871 
1872  /* Generate the shapes */
1873  np=0;
1874  atomID=0;
1875  for(lID=0;lID<r->nl;lID++)
1876  {
1877  if (LinkCanCollide(lID,r->nl,no,checkCollisionsLL,checkCollisionsLO))
1878  {
1879  l=GetMechanismLink(lID,m);
1880 
1881  if (!IsGroundLink(lID))
1882  r->objID[lID]=StartRigidCLLObject(r->r);
1883  else
1884  r->objID[lID]=0;
1885  added=FALSE;
1886 
1887  nB=LinkNBodies(l);
1888 
1889  for(bID=0;bID<nB;bID++)
1890  {
1891  if (GetLinkBodyStatus(bID,l)!=DECOR_SHAPE)
1892  {
1893  shape=GetLinkBody(bID,l);
1894  if (GetPolyhedronType(shape)!=SPHERE)
1895  Error("The RigidCLL collision engine can only be used on spheres");
1896 
1897  GetPolyhedronCenter(center,shape);
1898  rad=GetPolyhedronRadius(shape);
1899 
1900  if (IsGroundLink(lID))
1901  AddFixedSphere2RigidCLL(atomID,center,rad,r->r);
1902  else
1903  AddMobileSphere2RigidCLL(atomID,center,rad,r->r);
1904  atomID++;
1905  added=TRUE;
1906  }
1907  }
1908 
1909  if (!added)
1910  Error("Empty link in InitRigidCLL");
1911 
1912  if (!IsGroundLink(lID))
1913  CloseRigidCLLObject(r->r);
1914  np++;
1915  }
1916  else
1917  r->objID[lID]=NO_UINT;
1918  }
1919  npl=np;
1920 
1921  /* molecular problems typically do not have 'environment' we
1922  treat them as rigids that will not move (will move with
1923  the identity transform).
1924  If we ever have 'environment' it will be composed by
1925  a list of spheres, each acting as a single rigid.*/
1926  if (ObstaclesCollideLikeGround(r->nl,no,checkCollisionsLL,checkCollisionsLO))
1927  {
1928  /* Add the obstacle spheres to the ground link (in case we have obstacles
1929  this will be the most usual case). */
1930  obsLikeGround=TRUE;
1931  for(oID=0;oID<no;oID++)
1932  {
1933  if (GetObstacleShapeStatus(oID,e)!=DECOR_SHAPE)
1934  {
1935  shape=GetObstacleShape(oID,e);
1936 
1937  if (GetPolyhedronType(shape)!=SPHERE)
1938  Error("The RigidCLL collision engine can only be used on spheres");
1939 
1940  GetPolyhedronCenter(center,shape);
1941  rad=GetPolyhedronRadius(shape);
1942 
1943  AddFixedSphere2RigidCLL(atomID,center,rad,r->r);
1944  atomID++;
1945  }
1946  }
1947  }
1948  else
1949  {
1950  /* add the obstacles spheres individually */
1951  obsLikeGround=FALSE;
1952  for(oID=0;oID<no;oID++)
1953  {
1954  if ((ObstacleCanCollide(oID,r->nl,checkCollisionsLO))&&
1956  {
1957  shape=GetObstacleShape(oID,e);
1958 
1959  if (GetPolyhedronType(shape)!=SPHERE)
1960  Error("The RigidCLL collision engine can only be used on spheres");
1961 
1962  r->objID[r->nl+oID]=StartRigidCLLObject(r->r);
1963 
1964  GetPolyhedronCenter(center,shape);
1965  rad=GetPolyhedronRadius(shape);
1966 
1967  AddMobileSphere2RigidCLL(atomID,center,rad,r->r);
1968  atomID++;
1969 
1970  CloseRigidCLLObject(r->r);
1971  np++;
1972  }
1973  }
1974  }
1975 
1976  /* init the space for the transforms */
1977  NEW(r->t,np,THTransform);
1978  /* the ground link does not move */
1979  HTransformIdentity(&(r->t[0]));
1980  /* The obstacles will never move */
1981  if (!obsLikeGround)
1982  {
1983  for(no=npl;no<np;no++)
1984  HTransformIdentity(&(r->t[no]));
1985  }
1986 
1987  /* Now define the possible collisions */
1988  for(lID1=0;lID1<r->nl;lID1++)
1989  {
1990  for(lID2=lID1+1;lID2<r->nl;lID2++)
1991  {
1992  if (checkCollisionsLL[lID1][lID2])
1993  ActivateCollisionsRigidCLL(r->objID[lID1],r->objID[lID2],r->r);
1994  else
1995  DeactivateCollisionsRigidCLL(r->objID[lID1],r->objID[lID2],r->r);
1996  }
1997  if (!obsLikeGround)
1998  {
1999  for(oID=0;oID<no;oID++)
2000  {
2001  if (checkCollisionsLO[lID1][oID])
2002  ActivateCollisionsRigidCLL(r->objID[lID1],r->objID[r->nl+oID],r->r);
2003  else
2004  DeactivateCollisionsRigidCLL(r->objID[lID1],r->objID[r->nl+oID],r->r);
2005  }
2006  }
2007  }
2008  free(center);
2009  }
2010 
2011  boolean CheckCollisionRigidCLL(THTransform *tl,TrigidCLLCD *r)
2012  {
2013  unsigned int i;
2014 
2015  for(i=0;i<r->nl;i++)
2016  {
2017  if (r->objID[i]!=NO_UINT)
2018  HTransformCopy(&(r->t[r->objID[i]]),&(tl[i]));
2019  }
2020 
2021  return(MoveAndCheckRigidCLL(r->t,r->r));
2022  }
2023 
2024  void DeleteRigidCLLCD(TrigidCLLCD *r)
2025  {
2026  DeleteRigidCLL(r->r);
2027  free(r->t);
2028  free(r->objID);
2029  }
2030 
2031 #endif
2032 
2033 
2034 /************************************************************************************************/
2035 /* Private functions: generic functions on arrays of detected collisions. Only some engines
2036  are able to generate such arrays (SOLID and FCL) */
2037 
2038 void StoreCollisionInfoInt(FILE *f,char *fname,unsigned int objectID,Tmechanism *m,unsigned int nc,TCollisionInfo *c)
2039 {
2040  if (nc>0)
2041  {
2042  unsigned int i,j;
2043  unsigned int lID1,lID2;
2044  boolean isLink1,isLink2;
2045  THTransform *t1,*t2;
2046  char *ln1,*ln2;
2047  char *wname="world";
2048 
2049  fprintf(f,"fname='%s';\n\n",fname);
2050  for(i=0;i<nc;i++)
2051  {
2052  /* We are only interested on collisions with respect to the manipulated object (which actually is a link) */
2053  if ((objectID==NO_UINT)||
2054  ((c[i].isLink1)&&(c[i].id1==objectID))||
2055  ((c[i].isLink2)&&(c[i].id2==objectID)))
2056  {
2057  j=i+1; /* printed indices start at 1 */
2058 
2059  if ((objectID==NO_UINT)||((c[i].isLink1)&&(c[i].id1==objectID)))
2060  {
2061  isLink1=c[i].isLink1;
2062  isLink2=c[i].isLink2;
2063  lID1=c[i].id1;
2064  lID2=c[i].id2;
2065  t1=&(c[i].t1);
2066  t2=&(c[i].t2);
2067  }
2068  else
2069  {
2070  isLink1=c[i].isLink2;
2071  isLink2=c[i].isLink1;
2072  lID1=c[i].id2;
2073  lID2=c[i].id1;
2074  t1=&(c[i].t2);
2075  t2=&(c[i].t1);
2076  }
2077 
2078  ln1=(isLink1?GetLinkName(GetMechanismLink(lID1,m)):wname);
2079  ln2=(isLink2?GetLinkName(GetMechanismLink(lID2,m)):wname);
2080 
2081  fprintf(f,"p_%u=[%.16f %.16f %.16f 1]';\n\n",j,c[i].point[0],c[i].point[1],c[i].point[2]);
2082 
2083  fprintf(f,"T1_%u=[",j);
2084  HTransformPrint(f,t1);
2085  fprintf(f,"];\nlname1_%u='%s';\n\n",j,ln1);
2086 
2087  fprintf(f,"T2_%u=[",j);
2088  HTransformPrint(f,t2);
2089  fprintf(f,"];\nlname2_%u='%s';\n\n\n\n",j,ln2);
2090  }
2091  }
2092  for(i=0;i<nc;i++)
2093  {
2094  j=i+1;
2095  fprintf(f,"GenerateContactEqs(%u,fname,p_%u,lname1_%u,T1_%u,lname2_%u,T2_%u);\n",j,j,j,j,j,j);
2096  }
2097  fprintf(f,"\n\n\nexit\n");
2098  }
2099 }
2100 
2101 
2103  unsigned int nc,TCollisionInfo *c)
2104 {
2105  char *ln1,*ln2;
2106  unsigned int i;
2107  double p1[3],p2[3];
2108  THTransform t1,t2;
2109 
2110  /* if we stored any the collision information */
2111  for(i=0;i<nc;i++)
2112  {
2113  ln1=(c[i].isLink1?GetLinkName(GetMechanismLink(c[i].id1,m)):GetObstacleName(c[i].id1,e));
2114  ln2=(c[i].isLink2?GetLinkName(GetMechanismLink(c[i].id2,m)):GetObstacleName(c[i].id2,e));
2115 
2116  /* Transform the contact point given in global coordinates to local
2117  coordinates of the two involved bodies*/
2118  HTransformInverse(&(c[i].t1),&t1);
2119  HTransformApply(c[i].point,p1,&t1);
2120 
2121  HTransformInverse(&(c[i].t2),&t2);
2122  HTransformApply(c[i].point,p2,&t2);
2123 
2124  fprintf(stderr," - %s and %s collide at (%.16f,%.16f,%.16f) (%.16f,%.16f,%.16f)\n",
2125  ln1,ln2,p1[0],p1[1],p1[2],p2[0],p2[1],p2[2]);
2126 
2127  HTransformDelete(&t1);
2128  HTransformDelete(&t2);
2129  }
2130 }
2131 
2132 /************************************************************************************************/
2133 
2134 
2135 void InitCD(unsigned int engine,boolean parallel,
2136  Tmechanism *m,Tenvironment *e,
2137  boolean **checkCollisionsLL,boolean **checkCollisionsLO,
2138  TworldCD *cd)
2139 {
2140  cd->solid=NULL;
2141  cd->vcollide=NULL;
2142  cd->pqp=NULL;
2143  cd->fcl=NULL;
2144  cd->rigidCLL=NULL;
2145 
2146  switch(engine)
2147  {
2148  case SOLID:
2149  #ifdef _HAVE_SOLID
2150  NEW(cd->solid,1,TsolidCD);
2151  InitSolidCD(parallel,m,e,checkCollisionsLL,checkCollisionsLO,cd->solid);
2152  cd->engine=SOLID;
2153  #else
2154  Error("SOLID collision detection engine is not available");
2155  #endif
2156  break;
2157 
2158  case VCOLLIDE:
2159  #ifdef _HAVE_VCOLLIDE
2160  NEW(cd->vcollide,1,TvcollideCD);
2161  InitVcollideCD(m,e,checkCollisionsLL,checkCollisionsLO,cd->vcollide);
2162  cd->engine=VCOLLIDE;
2163  #else
2164  Error("VCOLLIDE collision detection engine is not available");
2165  #endif
2166  break;
2167 
2168  case PQP:
2169  #ifdef _HAVE_PQP
2170  NEW(cd->pqp,1,TpqpCD);
2171  InitPQPCD(m,e,checkCollisionsLL,checkCollisionsLO,cd->pqp);
2172  cd->engine=PQP;
2173  #else
2174  Error("PQP collision detection engine is not available");
2175  #endif
2176  break;
2177 
2178  case FCL:
2179  case C_FCL:
2180  #ifdef _HAVE_FCL
2181  NEW(cd->fcl,1,TfclCD);
2182  InitFCLCD(m,e,checkCollisionsLL,checkCollisionsLO,cd->fcl);
2183  cd->engine=engine;
2184  #else
2185  Error("FCL collision detection engine is not available");
2186  #endif
2187  break;
2188 
2189  case BULLET:
2190  case C_BULLET:
2191  #ifdef _HAVE_BULLET
2192  NEW(cd->bullet,1,TBulletCD);
2193  InitBulletCD(m,e,checkCollisionsLL,checkCollisionsLO,(engine==C_BULLET),cd->bullet);
2194  cd->engine=engine;
2195  #else
2196  Error("BULLET collision detection engine is not available");
2197  #endif
2198  break;
2199 
2200  case RIGIDCLL:
2201  #ifdef _HAVE_RIGIDCLL
2202  NEW(cd->rigidCLL,1,TrigidCLLCD);
2203  InitRigidCLLCD(m,e,checkCollisionsLL,checkCollisionsLO,cd->rigidCLL);
2204  cd->engine=RIGIDCLL;
2205  #else
2206  Error("RigidCLL collision detection engine is not available");
2207  #endif
2208  break;
2209 
2210  case NOCD:
2211  break;
2212 
2213  default:
2214  Error("Undefined collision detection engine");
2215  }
2216 }
2217 
2218 boolean LinkCanCollide(unsigned int l,unsigned int nl,unsigned int no,
2219  boolean **checkCollisionsLL,boolean **checkCollisionsLO)
2220 {
2221  unsigned int i;
2222  boolean found;
2223 
2224  found=FALSE;
2225  i=0;
2226  while((!found)&&(i<nl))
2227  {
2228  found=checkCollisionsLL[l][i];
2229  i++;
2230  }
2231  i=0;
2232  while((!found)&&(i<no))
2233  {
2234  found=checkCollisionsLO[l][i];
2235  i++;
2236  }
2237  return(found);
2238 }
2239 
2240 boolean ObstacleCanCollide(unsigned int o,unsigned int nl,boolean **checkCollisionsLO)
2241 {
2242  unsigned int i;
2243  boolean found;
2244 
2245  found=FALSE;
2246  i=0;
2247  while((!found)&&(i<nl))
2248  {
2249  found=checkCollisionsLO[i][o];
2250  i++;
2251  }
2252  return(found);
2253 }
2254 
2255 unsigned int GetCDEngine(TworldCD *cd)
2256 {
2257  return(cd->engine);
2258 }
2259 
2260 boolean CheckCollision(boolean all,
2261  THTransform *tl,TLinkConf *def,
2262  THTransform *tlPrev,TLinkConf *defPrev,
2263  TworldCD *cd)
2264 {
2265  boolean collision=FALSE;
2266 
2267  switch (cd->engine)
2268  {
2269  case SOLID:
2270  #ifdef _HAVE_SOLID
2271  collision=CheckCollisionSolid(all,tl,def,cd->solid);
2272  #else
2273  Error("SOLID collision detection engine is not available");
2274  #endif
2275  break;
2276 
2277  case VCOLLIDE:
2278  #ifdef _HAVE_VCOLLIDE
2279  /* flag 'all' is not honored */
2280  collision=CheckCollisionVcollide(tl,cd->vcollide);
2281  #else
2282  Error("VCOLLIDE collision detection engine is not available");
2283  #endif
2284  break;
2285 
2286  case PQP:
2287  #ifdef _HAVE_PQP
2288  /* flag 'all' is not honored */
2289  collision=CheckCollisionPQP(tl,cd->pqp);
2290  #else
2291  Error("PQP collision detection engine is not available");
2292  #endif
2293  break;
2294 
2295  case FCL:
2296  #ifdef _HAVE_FCL
2297  collision=CheckCollisionFCL(all,tl,cd->fcl);
2298  #else
2299  Error("FCL collision detection engine is not available");
2300  #endif
2301  break;
2302 
2303  case C_FCL:
2304  #ifdef _HAVE_FCL
2305  collision=CheckContCollisionFCL(tl,tlPrev,cd->fcl);
2306  #else
2307  Error("FCL collision detection engine is not available");
2308  #endif
2309  break;
2310 
2311  case BULLET:
2312  #ifdef _HAVE_BULLET
2313  collision=CheckCollisionBullet(all,tl,cd->bullet);
2314  #else
2315  Error("BULLET collision detection engine is not available");
2316  #endif
2317  break;
2318 
2319  case C_BULLET:
2320  #ifdef _HAVE_BULLET
2321  collision=CheckContCollisionBullet(tl,tlPrev,cd->bullet);
2322  #else
2323  Error("BULLET collision detection engine is not available");
2324  #endif
2325  break;
2326 
2327  case RIGIDCLL:
2328  #ifdef _HAVE_RIGIDCLL
2329  /* flag 'all' is not honored */
2330  collision=CheckCollisionRigidCLL(tl,cd->rigidCLL);
2331  #else
2332  Error("RigidCLL collision detection engine is not available");
2333  #endif
2334  break;
2335 
2336  case NOCD:
2337  break;
2338 
2339  default:
2340  Error("Undefined collision detection engine");
2341  }
2342 
2343  return(collision);
2344 }
2345 
2346 void StoreCollisionInfo(FILE *f,char *fname,unsigned int objectID,Tmechanism *m,TworldCD *cd)
2347 {
2348  switch (cd->engine)
2349  {
2350  case SOLID:
2351  #ifdef _HAVE_SOLID
2352  StoreCollisionInfoInt(f,fname,objectID,m,cd->solid->nc,cd->solid->collision);
2353  #else
2354  Error("SOLID collision detection engine is not available");
2355  #endif
2356  break;
2357  case FCL:
2358  #ifdef _HAVE_FCL
2359  StoreCollisionInfoInt(f,fname,objectID,m,cd->fcl->nc,cd->fcl->collision);
2360  #else
2361  Error("FCL collision detection engine is not available");
2362  #endif
2363  break;
2364  case BULLET:
2365  #ifdef _HAVE_BULLET
2366  StoreCollisionInfoInt(f,fname,objectID,m,cd->bullet->nc,cd->bullet->collision);
2367  #else
2368  Error("Bullet collision detection engine is not available");
2369  #endif
2370  break;
2371  }
2372 }
2373 
2375 {
2376  switch (cd->engine)
2377  {
2378  case SOLID:
2379  #ifdef _HAVE_SOLID
2380  PrintCollisionInfoInt(tl,m,e,cd->solid->nc,cd->solid->collision);
2381  #else
2382  Error("SOLID collision detection engine is not available");
2383  #endif
2384  break;
2385  case FCL:
2386  #ifdef _HAVE_FCL
2387  PrintCollisionInfoInt(tl,m,e,cd->fcl->nc,cd->fcl->collision);
2388  #else
2389  Error("FCL collision detection engine is not available");
2390  #endif
2391  break;
2392  case BULLET:
2393  #ifdef _HAVE_BULLET
2394  PrintCollisionInfoInt(tl,m,e,cd->bullet->nc,cd->bullet->collision);
2395  #else
2396  Error("Bullet collision detection engine is not available");
2397  #endif
2398  break;
2399  }
2400 }
2401 
2402 
2404 {
2405  switch (cd->engine)
2406  {
2407  case SOLID:
2408  #ifdef _HAVE_SOLID
2409  DeleteSolidCD(cd->solid);
2410  free(cd->solid);
2411  #endif
2412  cd->solid=NULL;
2413  break;
2414 
2415  case VCOLLIDE:
2416  #ifdef _HAVE_VCOLLIDE
2418  free(cd->vcollide);
2419  #endif
2420  cd->vcollide=NULL;
2421  break;
2422 
2423  case PQP:
2424  #ifdef _HAVE_PQP
2425  DeletePQPCD(cd->pqp);
2426  free(cd->pqp);
2427  #endif
2428  cd->pqp=NULL;
2429  break;
2430 
2431  case FCL:
2432  case C_FCL:
2433  #ifdef _HAVE_FCL
2434  DeleteFCLCD(cd->fcl);
2435  free(cd->fcl);
2436  #endif
2437  cd->fcl=NULL;
2438  break;
2439 
2440  case BULLET:
2441  case C_BULLET:
2442  #ifdef _HAVE_BULLET
2443  DeleteBulletCD(cd->bullet);
2444  free(cd->bullet);
2445  #endif
2446  cd->bullet=NULL;
2447  break;
2448 
2449  case RIGIDCLL:
2450  #ifdef _HAVE_RIGIDCLL
2451  DeleteRigidCLLCD(cd->rigidCLL);
2452  free(cd->rigidCLL);
2453  #endif
2454  cd->rigidCLL=NULL;
2455  break;
2456 
2457  case NOCD:
2458  break;
2459 
2460  default:
2461  Error("Undefined collision detection engine (d)");
2462  }
2463 }
2464 
TsolidCD * solid
Definition: cd.h:263
boolean SolidCorrection(THTransform *t, Tpolyhedron *p)
Returns a transform that corrects the position of a polyhedron.
Definition: cd.c:584
unsigned int id2
Definition: cd.h:65
CBLAS_INLINE void ScaleVector(double f, unsigned int s, double *v)
Scales a vector.
Definition: basic_algebra.c:30
double GetPolyhedronRadius(Tpolyhedron *p)
Returns the radius used in the definition of the object.
Definition: polyhedron.c:1440
unsigned int linkID
Definition: cd.h:80
void * bullet
Definition: cd.h:289
#define PQP
One of the possible collison detection engines.
Definition: parameters.h:87
Information on the detected collisions.
Definition: cd.h:60
void HTransformRz2(double s, double c, THTransform *t)
Constructor.
Definition: htransform.c:232
boolean LinkCanCollide(unsigned int l, unsigned int nl, unsigned int no, boolean **checkCollisionsLL, boolean **checkCollisionsLO)
Identifies links than can collide.
Definition: cd.c:2218
boolean isLink2
Definition: cd.h:64
#define FALSE
FALSE.
Definition: boolean.h:30
void * fcl
Definition: cd.h:283
void HTransformApply(double *p_in, double *p_out, THTransform *t)
Multiply a homogeneous transform and a vector.
Definition: htransform.c:782
void HTransformTxyz(double tx, double ty, double tz, THTransform *t)
Constructor.
Definition: htransform.c:146
unsigned int * bodyID
Definition: cd.h:130
TCollisionInfo * collision
Definition: cd.h:113
DT_RespTableHandle respTable
Definition: cd.h:105
void GetPolyhedronCenter(double *c, Tpolyhedron *p)
Gets the center of the spheres.
Definition: polyhedron.c:1353
#define NOCD
One of the possible collison detection engines.
Definition: parameters.h:141
#define NEW(_var, _n, _type)
Allocates memory space.
Definition: defines.h:385
Auxiliary data for the collision detection.
Definition: cd.h:258
void HTransformTz(double tz, THTransform *t)
Constructor.
Definition: htransform.c:134
Information associated with the PQP collision detection engine.
Definition: cd.h:144
unsigned int AddPolyhedron2Vcollide(double **v, unsigned int nf, unsigned int *nvf, unsigned int **fv, Tvcollide *vc)
Adds a polyhedron to the vcollide object.
Definition: cd_vcollide.cpp:30
A homgeneous transform in R^3.
double point[3]
Definition: cd.h:67
Tvcollide * InitVcollide()
Initializes a Vcollide object.
Definition: cd_vcollide.cpp:21
void InitVcollideCD(Tmechanism *m, Tenvironment *e, boolean **checkCollisionsLL, boolean **checkCollisionsLO, TvcollideCD *vc)
Initializes the Vcollide collision engine.
Definition: cd.c:861
void StoreCollisionInfoInt(FILE *f, char *fname, unsigned int objectID, Tmechanism *m, unsigned int nc, TCollisionInfo *c)
Stores the information collected during last collision check into a file.
Definition: cd.c:2038
Information associated with the solid collision detection engine.
Definition: cd.h:103
void DeleteSolidCD(TsolidCD *s)
Deletes the Solid related collision information.
Definition: cd.c:835
CBLAS_INLINE double Norm(unsigned int s, double *v)
Computes the norm of a vector.
unsigned int GetMechanismNLinks(Tmechanism *m)
Gets the number of links of a mechanism.
Definition: mechanism.c:26
boolean correction
Definition: cd.h:93
void InitCD(unsigned int engine, boolean parallel, Tmechanism *m, Tenvironment *e, boolean **checkCollisionsLL, boolean **checkCollisionsLO, TworldCD *cd)
Initializes the collision detector.
Definition: cd.c:2135
boolean CheckCollision(boolean all, THTransform *tl, TLinkConf *def, THTransform *tlPrev, TLinkConf *defPrev, TworldCD *cd)
Determines if there is a collision.
Definition: cd.c:2260
TvcollideCD * vcollide
Definition: cd.h:269
#define TRUE
TRUE.
Definition: boolean.h:21
CBLAS_INLINE void Normalize(unsigned int s, double *v)
Normalizes a vector.
#define CYLINDER
One of the possible type of polyhedrons.
Definition: polyhedron.h:80
unsigned int * linkID
Definition: cd.h:146
CBLAS_INLINE void HTransform2GLMatrix(double *m, THTransform *t)
Defines a GL column-major matrix from a homogeneous transform.
Definition: htransform.c:357
void Error(const char *s)
General error function.
Definition: error.c:80
#define RIGIDCLL
One of the possible collison detection engines.
Definition: parameters.h:132
unsigned int * vcID
Definition: cd.h:131
#define OFF
One of the possible type of polyhedrons.
Definition: polyhedron.h:59
A mechanism description.
Definition: mechanism.h:51
#define SEGMENTS
One of the possible type of polyhedrons.
Definition: polyhedron.h:100
TpqpCD * pqp
Definition: cd.h:275
boolean simple
Definition: cd.h:108
CBLAS_INLINE void HTransformProduct(THTransform *t1, THTransform *t2, THTransform *t3)
Product of two homogeneous transforms.
Definition: htransform.c:410
#define ZERO
Floating point operations giving a value below this constant (in absolute value) are considered 0...
Definition: defines.h:37
#define LINE
One of the possible type of polyhedrons.
Definition: polyhedron.h:90
void HTransformX2Vect(double sy, double sz, double *p1, double *p2, THTransform *t)
Transform a unitary vector along the X axis to a generic vector.
Definition: htransform.c:574
unsigned int * bodyID
Definition: cd.h:147
unsigned int GetObstacleShapeStatus(unsigned int i, Tenvironment *e)
Gets the status (NORMAL, HIDDEN, DECOR) of an obstacle given its identifier.
Definition: environment.c:96
A polyhedron.
Definition: polyhedron.h:134
Error and warning functions.
Tpqp ** model
Definition: cd.h:148
DT_Bool CDCallBackInfo(void *client_data, void *obj1, void *obj2, const DT_CollData *cd)
Callback called whenever a collision is detected.
Definition: cd.c:455
unsigned int np
Definition: cd.h:106
void PrintCollisionInfoInt(THTransform *tl, Tmechanism *m, Tenvironment *e, unsigned int nc, TCollisionInfo *c)
Prints information about the last collision check.
Definition: cd.c:2102
#define FCL
One of the possible collison detection engines.
Definition: parameters.h:96
boolean VcollideTest(Tvcollide *vc)
Test for collision.
Definition: cd_vcollide.cpp:87
Information of each object.
Definition: cd.h:78
void HTransformInverse(THTransform *t, THTransform *ti)
Inverse of a homogeneous transform.
Definition: htransform.c:503
THTransform t1
Definition: cd.h:69
void * rigidCLL
Definition: cd.h:295
boolean ObstacleCanCollide(unsigned int o, unsigned int nl, boolean **checkCollisionsLO)
Identifies obstacles than can collide.
Definition: cd.c:2240
unsigned int id1
Definition: cd.h:62
A collection of obstacles (convex polyhedrons) with their names.
Definition: environment.h:39
void GetOFFInfo(unsigned int *nv, double ***v, unsigned int *nf, unsigned int **nvf, unsigned int ***fv, Tpolyhedron *p)
Gets the OFF information.
Definition: polyhedron.c:1383
DT_ObjectHandle handler
Definition: cd.h:85
Definitions of constants and macros used in several parts of the cuik library.
void DeleteVcollideObject(unsigned int o, Tvcollide *vc)
Deletes a given vcollide object.
unsigned int nc
Definition: cd.h:111
DT_ShapeHandle AddShape2Solid(Tpolyhedron *p)
Adds a shape to the collision detection data.
Definition: cd.c:524
double normal[3]
Definition: cd.h:68
unsigned int * p2
Definition: cd.h:151
#define C_FCL
One of the possible collison detection engines.
Definition: parameters.h:105
void InitSolidCD(boolean parallel, Tmechanism *m, Tenvironment *e, boolean **checkCollisionsLL, boolean **checkCollisionsLO, TsolidCD *s)
Initializes the Solid collision engine.
Definition: cd.c:627
unsigned int id
Definition: cd.h:79
#define VCOLLIDE
One of the possible collison detection engines.
Definition: parameters.h:78
void PrintCollisionInfo(THTransform *tl, Tmechanism *m, Tenvironment *e, TworldCD *cd)
Prints some information collected during last collision check.
Definition: cd.c:2374
Tpqp * DefinePQPModel(double **v, unsigned int nf, unsigned int *nvf, unsigned int **fv)
Adds a polyhedron to the pqp object.
Definition: cd_pqp.cpp:20
DT_ResponseClass respClass
Definition: cd.h:86
void DeleteCD(TworldCD *cd)
Collision information destructor.
Definition: cd.c:2403
Tvcollide * vc
Definition: cd.h:127
void StoreCollisionInfo(FILE *f, char *fname, unsigned int objectID, Tmechanism *m, TworldCD *cd)
Stores the information collected during last collision check into a file.
Definition: cd.c:2346
#define SPHERE
One of the possible type of polyhedrons.
Definition: polyhedron.h:70
boolean isLink1
Definition: cd.h:61
unsigned int GetPolyhedronStatus(Tpolyhedron *p)
Gets the status of a polyhedron (NORMAL, HIDDEN, DECOR).
Definition: polyhedron.c:1378
#define MEM_DUP(_var, _n, _type)
Duplicates a previously allocated memory space.
Definition: defines.h:414
unsigned int * p1
Definition: cd.h:150
#define NO_UINT
Used to denote an identifier that has not been initialized.
Definition: defines.h:435
CBLAS_INLINE void SumVectorScale(unsigned int s, double *v1, double w, double *v2, double *v)
Adds two vectors with a scale.
Definition: basic_algebra.c:86
DT_ShapeHandle shape
Definition: cd.h:84
CBLAS_INLINE void SubtractVector(unsigned int s, double *v1, double *v2)
Substracts a vector from another vector.
unsigned int GetCDEngine(TworldCD *cd)
Determines the collision engine.
Definition: cd.c:2255
void HTransformTx(double tx, THTransform *t)
Constructor.
Definition: htransform.c:112
boolean CheckCollisionSolid(boolean all, THTransform *tl, TLinkConf *def, TsolidCD *s)
Determines if there is a collision using the SOLID collision engine.
Definition: cd.c:778
void DeleteVcollide(Tvcollide *vc)
Deletes a vcollide object.
void MoveVcollideObject(unsigned int o, THTransform *t, Tvcollide *vc)
Moves a given v-collide object.
Definition: cd_vcollide.cpp:78
char * GetObstacleName(unsigned int i, Tenvironment *e)
Gets the name of an obstacle given its identifier.
Definition: environment.c:74
Information associated with the Vcollide collision detection engine.
Definition: cd.h:126
unsigned int mc
Definition: cd.h:112
void ActivateCollisionsVcollide(unsigned int o1, unsigned int o2, Tvcollide *vc)
Activates the collision between a pair of objects.
Definition: cd_vcollide.cpp:60
void DeleteVcollideCD(TvcollideCD *vc)
Deletes the Vcollide related collision information.
Definition: cd.c:974
void HTransformDelete(THTransform *t)
Destructor.
Definition: htransform.c:887
unsigned int engine
Definition: cd.h:260
boolean CheckCollisionPQP(THTransform *tl, TpqpCD *p)
Determines if there is a collision using the PQP collision engine.
Definition: cd.c:1093
void GetPolyhedronDefiningPoint(unsigned int i, double *point, Tpolyhedron *p)
Gets a point defining a a object.
Definition: polyhedron.c:1398
Headers of the interface with the collision detection engine.
void DeletePQPCD(TpqpCD *p)
Deletes the PQP related collision information.
Definition: cd.c:1126
#define C_BULLET
One of the possible collison detection engines.
Definition: parameters.h:123
DT_SceneHandle scene
Definition: cd.h:104
Tlink * GetMechanismLink(unsigned int i, Tmechanism *m)
Gets a link given its identifier.
Definition: mechanism.c:149
unsigned int * linkID
Definition: cd.h:129
boolean PQPTest(THTransform *t1, Tpqp *m1, THTransform *t2, Tpqp *m2)
Test for collision.
Definition: cd_pqp.cpp:50
void HTransformTy(double ty, THTransform *t)
Constructor.
Definition: htransform.c:123
void InitPQPCD(Tmechanism *m, Tenvironment *e, boolean **checkCollisionsLL, boolean **checkCollisionsLO, TpqpCD *p)
Initializes the PQP collision engine.
Definition: cd.c:992
unsigned int bodyID
Definition: cd.h:81
void HTransformCopy(THTransform *t_dst, THTransform *t_src)
Copy constructor.
Definition: htransform.c:83
unsigned int np
Definition: cd.h:128
#define BULLET
One of the possible collison detection engines.
Definition: parameters.h:114
TsolidObj * object
Definition: cd.h:107
Tpolyhedron * GetObstacleShape(unsigned int i, Tenvironment *e)
Gets the convex polyhedron of an obstacle given its identifier.
Definition: environment.c:85
boolean CheckCollisionVcollide(THTransform *tl, TvcollideCD *vc)
Determines if there is a collision using the VCOLLIDE collision engine.
Definition: cd.c:958
void DeactivateCollisionsVcollide(unsigned int o1, unsigned int o2, Tvcollide *vc)
Deactivates the collisions between a pair of object.
Definition: cd_vcollide.cpp:69
void Tpqp
The PQP object.
Definition: cd_pqp.h:33
void HTransformPrint(FILE *f, THTransform *t)
Prints the a homogeneous transform to a file.
Definition: htransform.c:822
THTransform t2
Definition: cd.h:70
void HTransformIdentity(THTransform *t)
Constructor.
Definition: htransform.c:69
unsigned int np
Definition: cd.h:145
void HTransformRy2(double s, double c, THTransform *t)
Constructor.
Definition: htransform.c:222
unsigned int nPairs
Definition: cd.h:149
unsigned int GetPolyhedronType(Tpolyhedron *p)
Retrives the type of a polyhedron.
Definition: polyhedron.c:1363
THTransform t
Definition: cd.h:87
unsigned int GetMechanismNBodies(Tmechanism *m)
Gets the number of convex sub-parts (or bodies) of a mechanism.
Definition: mechanism.c:36
#define SOLID
One of the possible collison detection engines.
Definition: parameters.h:69
#define DECOR_SHAPE
One of the possible type of shapes.
Definition: polyhedron.h:46
unsigned int GetEnvironmentNObstacles(Tenvironment *e)
Gets the number of obstacles in the environment.
Definition: environment.c:43
void DeletePQPModel(Tpqp *m)
Deletes a given PQP object.
Definition: cd_pqp.cpp:88
Link configuration.
Definition: link.h:276
THTransform tc
Definition: cd.h:90
boolean HTransformIsIdentity(THTransform *t)
Identify the identity matrix.
Definition: htransform.c:91