link.c
Go to the documentation of this file.
1 #include "link.h"
2 #include "polyhedron.h"
3 #include "varnames.h"
4 #include "basic_algebra.h"
5 #include "geom.h"
6 
7 #include <math.h>
8 #include <string.h>
9 
25 #define MAX_ROT_VARS 14
26 
38 
59 void GetRotVarID(unsigned int r,TCuikSystem *cs,unsigned int *vID,Tlink *l);
60 
75 
87 
99 
110 void GetRotVarIDQLinks(TCuikSystem *cs,Tlink *l);
111 
124 
137 void GenerateLinkRotAxisX(Tparameters *p,unsigned int lID,TCuikSystem *cs,Tlink *l);
138 
151 void GenerateLinkRotLinks(Tparameters *p,unsigned int lID,TCuikSystem *cs,Tlink *l);
152 
165 void GenerateLinkRotFLinks(Tparameters *p,unsigned int lID,TCuikSystem *cs,Tlink *l);
166 
179 void GenerateLinkRotQLinks(Tparameters *p,unsigned int lID,TCuikSystem *cs,Tlink *l);
180 
205 void ApplyLinkRotAxisX(double sf,unsigned int sv,double *p,Tequation *eq,
206  TCuikSystem *cs,boolean groundLink,Tlink *l);
207 
233 void ApplyLinkRotDeformX(Tparameters *pr,double sf,unsigned int sv,double *p,Tequation *eq,
234  TCuikSystem *cs,boolean groundLink,Tlink *l);
258 void ApplyLinkRotLinks(double sf,unsigned int sv,double *p,Tequation *eq,
259  TCuikSystem *cs,boolean groundLink,Tlink *l);
260 
284 void ApplyLinkRotFLinks(double sf,unsigned int sv,double *p,Tequation *eq,
285  TCuikSystem *cs,boolean groundLink,Tlink *l);
286 
310 void ApplyLinkRotQLinks(double sf,unsigned int sv,double *p,Tequation *eq,
311  TCuikSystem *cs,boolean groundLink,Tlink *l);
312 
331 void RegenerateLinkSolutionConf(unsigned int r,TCuikSystem *cs,double *sol,Tlink *l);
332 
351 void RegenerateLinkSolutionLinks(unsigned int r,TCuikSystem *cs,double *sol,boolean groundLink,Tlink *l);
352 
371 void RegenerateLinkSolutionQLinks(unsigned int r,TCuikSystem *cs,double *sol,boolean groundLink,Tlink *l);
372 
394 void RegenerateLinkBoxConf(unsigned int r,TCuikSystem *cs,Tbox *b,Tlink *l);
395 
414 void RegenerateLinkBoxLinks(unsigned int r,TCuikSystem *cs,Tbox *b,boolean groundLink,Tlink *l);
415 
434 void RegenerateLinkBoxQLinks(unsigned int r,TCuikSystem *cs,Tbox *b,boolean groundLink,Tlink *l);
435 
461 void GetTransform2LinkAxisX(unsigned int r,TCuikSystem *cs,double *sol,boolean groundLink,
462  double *trans,THTransform *t,Tlink *l);
463 
486 void GetTransform2LinkLinks(unsigned int r,TCuikSystem *cs,double *sol,boolean groundLink,
487  double *trans,THTransform *t,Tlink *l);
488 
510 void GetTransform2LinkFLinks(unsigned int r,TCuikSystem *cs,double *sol,boolean groundLink,
511  double *trans,THTransform *t,Tlink *l);
512 
535 void GetTransform2LinkQLinks(unsigned int r,TCuikSystem *cs,double *sol,boolean groundLink,
536  double *trans,THTransform *t,Tlink *l);
537 
553 void GenerateLinkSolutionAxisX(unsigned int r,THTransform *t,TCuikSystem *cs,
554  double *sol,boolean groundLink,Tlink *l);
555 
556 
569 void GenerateLinkSolutionLinks(unsigned int r,THTransform *t,TCuikSystem *cs,
570  double *sol,boolean groundLink,Tlink *l);
571 
584 void GenerateLinkSolutionFLinks(unsigned int r,THTransform *t,TCuikSystem *cs,
585  double *sol,boolean groundLink,Tlink *l);
586 
599 void GenerateLinkSolutionQLinks(unsigned int r,THTransform *t,TCuikSystem *cs,
600  double *sol,boolean groundLink,Tlink *l);
601 
617 void SetTransVars(boolean *vars,TCuikSystem *cs,Tlink *l);
618 
633 void SetRotVars(unsigned int r,boolean *vars,TCuikSystem *cs,Tlink *l);
634 
635 /**********************************************************************/
636 
637 
639 {
640  return(!IsVariableLengthLink(ld->l));
641 }
642 
643 void GetLinkConfTransform(unsigned int bID,THTransform *t,TLinkConf *ld)
644 {
645  if (EmptyLinkConf(ld))
647  else
648  {
649  if (GetLinkType(ld->l)==LINK_DeformX)
650  HTransformScaleX(ld->p[0],t);
651  else
652  {
653  /* Prismatic link: only the upper bodies are displaced */
654  if (bID>=(LinkNBodies(ld->l)/2))
655  HTransformTx(ld->p[0],t);
656  else
658  }
659  }
660 }
661 
663 {
664  if (ld->p!=NULL)
665  free(ld->p);
666 }
667 
668 /**********************************************************************/
669 
670 
671 void InitLink(char *name,Tlink *l)
672 {
673  unsigned int namel,i;
674 
675  namel=strlen(name);
676  if (namel==0)
677  l->name=NULL;
678  else
679  {
680  NEW(l->name,namel+1,char);
681  strcpy(l->name,name);
682  }
683 
684  l->type=LINK_FullRot; /* by default we encode the full rotation of the links. */
685 
687 
688  l->s=1;
689  l->c=0;
690  HTransformIdentity(&(l->R));
691  HTransformIdentity(&(l->iR));
692 
693  l->allSpheres=TRUE;
694 
695  for(i=0;i<3;i++)
696  l->axisID[i]=NO_UINT;
697 
698  /*sum of the maximum coordinate value for all bodies in the link*/
699  l->maxCoord=0.0;
700 
701  l->csvID=NULL;
702  l->nvID=0;
703  l->vID=NULL;
704 
705  /* In general links do not have to be in force equilibrium */
707 
708  /* force and fixedDim is only used in tensegrity structures */
709  l->extForce[0]=l->extForce[1]=l->extForce[2]=0.0;
710  l->fixedDim[0]=l->fixedDim[1]=l->fixedDim[2]=INF;
711 
712  /* Force-related information. Only used for some AxisX of DeformX links */
713  NewInterval(0,0,&(l->length));
714  l->varLength=FALSE;
715 
716  l->stiffness=0.0;
717  NewInterval(0,0,&(l->rest));
718  NewInterval(0,0,&(l->force));
719  l->forceModel=NO_FORCE;
720 }
721 
722 void InitNoRotLink(char *name,Tlink *l)
723 {
724  /* Initialize as a normal link, even if some structures will not be
725  actually used */
726  InitLink(name,l);
727  l->type=LINK_NoRot;
729 }
730 
732 {
733  if (l->forceModel==LINEAR_FORCE)
734  {
735  Tinterval r;
736  boolean reduction;
737 
738  do {
739  reduction=FALSE;
740 
741  /* f=k*(l-r) */
742  IntervalSubstract(&(l->length),&(l->rest),&r);
743  IntervalScale(&r,l->stiffness,&r);
744  if (!Intersection(&(l->force),&r,&r))
745  Error("Wrong force range");
746  else
747  {
748  if (IntervalSize(&(l->force))>IntervalSize(&r)+1e-6)
749  {
750  CopyInterval(&(l->force),&r);
751  reduction=TRUE;
752  }
753  }
754 
755  /* l=f/k+r */
756  if (fabs(l->stiffness)>ZERO)
757  {
758  IntervalScale(&(l->force),1.0/l->stiffness,&r);
759  IntervalAdd(&r,&(l->rest),&r);
760 
761  if (!Intersection(&(l->length),&r,&r))
762  Error("Wrong length range");
763  else
764  {
765  if (IntervalSize(&(l->length))>IntervalSize(&r)+ZERO)
766  {
767  CopyInterval(&(l->length),&r);
768  reduction=TRUE;
769  }
770  }
771  }
772 
773  /* r=l-f/k */
774  if (fabs(l->stiffness)>ZERO)
775  {
776  IntervalScale(&(l->force),1.0/l->stiffness,&r);
777  IntervalSubstract(&(l->length),&r,&r);
778 
779  if (!Intersection(&(l->rest),&r,&r))
780  Error("Wrong rest range");
781  else
782  {
783  if (IntervalSize(&(l->rest))>IntervalSize(&r)+ZERO)
784  {
785  CopyInterval(&(l->rest),&r);
786  reduction=TRUE;
787  }
788  }
789  }
790  } while (reduction);
791  }
792 }
793 
794 void InitAxisXLink(char *name,unsigned int forceModel,
795  Tinterval *length,Tinterval *force,Tlink *l)
796 {
797  /* Initialize as a normal link, even if some structures will not be
798  actually used */
799  InitLink(name,l);
800 
801  if (LowerLimit(length)<0)
802  Error("Wrong length in InitAxisXLink");
803  if (IntervalSize(length)>ZERO)
804  Error("The length of AxisX links must be constant");
805 
806  /* need the length of the leg, at least to print the link */
807  l->varLength=FALSE;
808  CopyInterval(&(l->length),length); /* actually a point */
809 
810  /* force model, if any, is not lineal */
811  l->stiffness=0.0;
812  NewInterval(0.0,0.0,&(l->rest));
813 
814  l->forceModel=forceModel;
815  switch(forceModel)
816  {
817  case NO_FORCE:
818  if (!ZeroInterval(force))
819  Error("Force must be zero for NO_FORCE links");
820  NewInterval(0.0,0.0,&(l->force));
821  break;
822  case VAR_FORCE:
823  CopyInterval(&(l->force),force);
824  break;
825  case LINEAR_FORCE:
826  Error("InitAxisXLink can not have linear force model (the length is constant)");
827  break;
828  default:
829  Error("Unknown force model in InitAxisXLink");
830  }
831 
832  l->type=LINK_AxisX;
833 }
834 
835 void InitDeformXLink(char *name,unsigned int forceModel,
836  Tinterval *length,double stiffness,
837  Tinterval *rest,Tinterval *force,Tlink *l)
838 {
839  /* Initialize as a normal link, even if some structures will not be
840  actually used */
841  InitLink(name,l);
842 
843  if (LowerLimit(length)<0)
844  Error("Wrong length in InitDeformXLink");
845 
846  /* set to variable length even if in some cases the length may be constant */
847  l->varLength=TRUE;
848  CopyInterval(&(l->length),length);
849 
850  l->forceModel=forceModel;
851  switch(forceModel)
852  {
853  case NO_FORCE:
854  if ((!ZeroInterval(force))||(fabs(stiffness)>ZERO)||(!ZeroInterval(rest)))
855  Error("NO_FORCE links do not use force, stiffness nor rest length");
856 
857  NewInterval(0.0,0.0,&(l->force));
858  l->stiffness=0.0;
859  NewInterval(0.0,0.0,&(l->rest));
860  break;
861  case VAR_FORCE:
862  if ((fabs(stiffness)>ZERO)||(!ZeroInterval(rest)))
863  Error("VAR_FORCE links do not use stiffness nor rest length");
864 
865  CopyInterval(&(l->force),force);
866  l->stiffness=0.0;
867  NewInterval(0.0,0.0,&(l->rest));
868  break;
869  case LINEAR_FORCE:
870  if (fabs(stiffness)<ZERO)
871  Error("Wrong linear force model stiffness parameter in InitDeformXLink");
872 
873  if (LowerLimit(rest)<0)
874  Error("Wrong rest in InitDeformXLink");
875  if (LowerLimit(rest)>UpperLimit(length))
876  Error("Wrong length/rest in InitDeformXLink");
877  if (IntervalSize(length)<ZERO)
878  Error("Force model can not be linear for constant length InitDeformXLink");
879 
880  CopyInterval(&(l->force),force);
881  l->stiffness=stiffness;
882  CopyInterval(&(l->rest),rest);
883 
885  break;
886  default:
887  Error("Unknown force model in InitDeformXLink");
888  }
889 
890  l->type=LINK_DeformX;
891 }
892 
893 void InitPrismaticXLink(char *name,unsigned int forceModel,
894  Tinterval *length,double stiffness,
895  Tinterval *rest,Tinterval *force,Tlink *l)
896 {
897  /* If there is no deformation -> use AxisX link */
898  if (IntervalSize(length)<ZERO)
899  {
900  if (forceModel==LINEAR_FORCE)
901  Error("Can not define linear force modesl on constant link prismatic legs");
902  if ((fabs(stiffness)>ZERO)||(!ZeroInterval(rest)))
903  Error("Wrong force parameters when defining a prismatic link");
904 
905  InitAxisXLink(name,forceModel,length,force,l);
906  }
907  else
908  {
909  /* Initialize as a normal link, even if some structures will not be
910  actually used */
911  InitLink(name,l);
912 
913  if (LowerLimit(length)<0)
914  Error("Wrong length in InitPrismaticXLink");
915 
916  if (LowerLimit(length)<IntervalSize(length))
917  Error("Wrong min-max length in InitPrismaticXLink");
918 
919  l->varLength=TRUE;
920  CopyInterval(&(l->length),length);
921 
922  l->forceModel=forceModel;
923  switch(forceModel)
924  {
925  case NO_FORCE:
926  if ((!ZeroInterval(force))||(fabs(stiffness)>ZERO)||(!ZeroInterval(rest)))
927  Error("NO_FORCE links do not use force, stiffness nor rest length");
928 
929  NewInterval(0.0,0.0,&(l->force));
930  l->stiffness=0.0;
931  NewInterval(0.0,0.0,&(l->rest));
932  break;
933  case VAR_FORCE:
934  if ((fabs(stiffness)>ZERO)||(!ZeroInterval(rest)))
935  Error("VAR_FORCE links do not use stiffness nor rest length");
936 
937  CopyInterval(&(l->force),force);
938  l->stiffness=0.0;
939  NewInterval(0.0,0.0,&(l->rest));
940  break;
941  case LINEAR_FORCE:
942  if (fabs(stiffness)<ZERO)
943  Error("Wrong linear force model stiffness parameter in InitPrismaticXLink");
944 
945  if (LowerLimit(rest)<0)
946  Error("Wrong rest in InitPrismaticXLink");
947  if (LowerLimit(rest)>UpperLimit(length))
948  Error("Wrong length/rest in InitPrismaticXLink");
949 
950  CopyInterval(&(l->force),force);
951  l->stiffness=stiffness;
952  CopyInterval(&(l->rest),rest);
953 
955  break;
956  default:
957  Error("Unknown force model in InitPrismaticXLink");
958  }
959 
961  }
962 }
963 
964 void CopyLink(Tlink *l_dst,Tlink *l_src)
965 {
966  unsigned int i,n;
967 
968  if (l_src==NULL)
969  l_dst->name=NULL;
970  else
971  {
972  NEW(l_dst->name,strlen(l_src->name)+1,char);
973  strcpy(l_dst->name,l_src->name);
974  }
975 
976  l_dst->type=l_src->type;
977 
978  InitVector(sizeof(double *),CopyVoidPtr,DeleteVoidPtr,INIT_NUM_SHAPES,&(l_dst->bodies));
979 
980  l_dst->allSpheres=TRUE;
981 
982  n=LinkNBodies(l_src);
983  for(i=0;i<n;i++)
984  AddBody2Link(GetLinkBody(i,l_src),l_dst);
985 
986  l_dst->c=l_src->c;
987  l_dst->s=l_src->s;
988  HTransformCopy(&(l_dst->R),&(l_src->R));
989  HTransformCopy(&(l_dst->iR),&(l_src->iR));
990 
991  for(i=0;i<3;i++)
992  l_dst->axisID[i]=l_src->axisID[i];
993 
994  l_dst->maxCoord=l_src->maxCoord;
995 
996  if (l_src->csvID==NULL)
997  {
998  l_dst->csvID=NULL;
999  l_dst->nvID=0;
1000  l_dst->vID=NULL;
1001  }
1002  else
1003  {
1004  l_dst->csvID=l_src->csvID;
1005  l_dst->nvID=l_src->nvID;
1006  NEW(l_dst->vID,l_dst->nvID,unsigned int);
1007  memcpy(l_dst->vID,l_src->vID,l_dst->nvID*sizeof(unsigned int));
1008  }
1009 
1010  l_dst->forceEquilibrium=l_src->forceEquilibrium;
1011 
1012  for(i=0;i<3;i++)
1013  {
1014  l_dst->extForce[i]=l_src->extForce[i];
1015  l_dst->fixedDim[i]=l_src->fixedDim[i];
1016  }
1017 
1018  CopyInterval(&(l_dst->length),&(l_src->length));
1019  l_dst->varLength=l_src->varLength;
1020 
1021  l_dst->stiffness=l_src->stiffness;
1022  CopyInterval(&(l_dst->rest),&(l_src->rest));
1023  CopyInterval(&(l_dst->force),&(l_src->force));
1024  l_dst->forceModel=l_src->forceModel;
1025 }
1026 
1028 {
1029  Tpolyhedron *bint;
1030 
1031  if (l->type==LINK_NoRot)
1032  Error("AddBody2Link can not be used for free-rotation links");
1033 
1034  if (!EmptyPolyhedron(b))
1035  {
1036  /* for LINK_AxisX only X-symmetric links make sense. */
1037  NEW(bint,1,Tpolyhedron);
1038  CopyPolyhedron(bint,b);
1039 
1040  NewVectorElement(&bint,&(l->bodies));
1041 
1042  l->allSpheres=((l->allSpheres)&&
1043  ((GetPolyhedronType(bint)==SPHERE)||
1044  (GetPolyhedronStatus(bint)==DECOR_SHAPE)));
1045 
1047  }
1048 }
1049 
1050 void AddForce2Link(double fx,double fy,double fz,Tlink *l)
1051 {
1052  l->extForce[0]+=fx;
1053  l->extForce[1]+=fy;
1054  l->extForce[2]+=fz;
1055 }
1056 
1057 double GetForceOnLink(unsigned int dim,Tlink *l)
1058 {
1059  if (dim>2)
1060  Error("Dimension out of range in GetForceOnLink");
1061  return(l->extForce[dim]);
1062 }
1063 
1065 {
1066  /* only possibly TRUE for NoRot links */
1068 }
1069 
1071 {
1072  return(l->forceEquilibrium);
1073 }
1074 
1075 void SetLinkTrans(unsigned int dim,double t,Tlink *l)
1076 {
1077  if (dim>2)
1078  Error("Dimension out of range in SetLinkTrans");
1079  l->fixedDim[dim]=t;
1080 }
1081 
1082 double GetLinkTrans(unsigned int dim,Tlink *l)
1083 {
1084  if (dim>2)
1085  Error("Dimension out of range in GetLinkTrans");
1086  return(l->fixedDim[dim]);
1087 }
1088 
1090 {
1091  return(&(l->length));
1092 }
1093 
1095 {
1096  if (l->forceModel!=LINEAR_FORCE)
1097  Error("Link without stiffness");
1098  return(l->stiffness);
1099 }
1100 
1102 {
1103  if (l->forceModel!=LINEAR_FORCE)
1104  Error("Link without rest length");
1105  return(&(l->rest));
1106 }
1107 
1109 {
1110  if (l->forceModel==NO_FORCE)
1111  Error("Link without force");
1112  return(&(l->force));
1113 }
1114 
1115 void ChangeLinkReferenceFrame(unsigned int r,double **p1,double **p2,Tlink *l)
1116 {
1117  if (l->type!=LINK_FullRot)
1118  Error("ChangeLinkReferenceFrame should only be used for full-rotation links");
1119 
1120  if ((r!=REP_QLINKS)&&(r!=REP_JOINTS))
1121  {
1122  /* This function has no effect when using quaternions. */
1123  double v[3][3],n1,n2;
1124  unsigned int i,j;
1125  double s,c,d,ac;
1126 
1127  /* Define the vectors from the points */
1128  n1=0;
1129  n2=0;
1130  for(i=0;i<3;i++)
1131  {
1132  v[0][i]=p1[1][i]-p1[0][i];
1133  v[1][i]=p2[1][i]-p2[0][i];
1134  n1+=v[0][i]*v[0][i];
1135  n2+=v[1][i]*v[1][i];
1136  }
1137  /* Normalize the vectors */
1138  n1=sqrt(n1);
1139  n2=sqrt(n2);
1140  for(i=0;i<3;i++)
1141  {
1142  v[0][i]/=n1;
1143  v[1][i]/=n2;
1144  }
1145 
1146  /*compute the cos/sin between the two vectors*/
1147  /*The dot product of the two vectors is the cosinus
1148  between the vectors*/
1149  c=0;
1150  for(i=0;i<3;i++)
1151  c+=v[0][i]*v[1][i];
1152  ac=fabs(c);
1153  if (ac<0.9) /*Ensure the two vectors are (by far) not aligned*/
1154  {
1155  /*The cross product of the two vectors is a vector
1156  orthogonal to the two vectors with norm the sinus
1157  of the angle between the vectors*/
1158  v[2][0]=v[0][1]*v[1][2]-v[0][2]*v[1][1];
1159  v[2][1]=v[0][2]*v[1][0]-v[0][0]*v[1][2];
1160  v[2][2]=v[0][0]*v[1][1]-v[0][1]*v[1][0];
1161 
1162  s=0;
1163  for(i=0;i<3;i++)
1164  s+=v[2][i]*v[2][i];
1165  s=sqrt(s);
1166  for(i=0;i<3;i++)
1167  v[2][i]/=s;
1168 
1169  l->c=c;
1170  l->s=s;
1171 
1172  /* Compute the inverse of the matrix with vector 'v' as colums
1173  (i.e., the inverse of the basis change).
1174  Rows of R are computed taking into account that
1175  R * [v1 v2 v3] = Id
1176  R=[w1;w2;w3]
1177  From construction w3=v3
1178  and w1=\alpha v1 + \beta v2
1179  w2=\gamma v1 + \delta v2
1180  where \alpa, \beta, \gamma, \delta are easy to find
1181  taking into account that w1*v1\tr=1 w1*v2\tr=0,...
1182  */
1183  d=(1/(s*s));
1184  for(j=0;j<3;j++)
1185  HTransformSetElement(0,j,d*(v[0][j]-c*v[1][j]),&(l->R));
1186  for(j=0;j<3;j++)
1187  HTransformSetElement(1,j,d*(v[1][j]-c*v[0][j]),&(l->R));
1188  for(j=0;j<3;j++)
1189  HTransformSetElement(2,j,v[2][j],&(l->R));
1190 
1191  /* iR: The inverse of R: v1 v2 v3 in colums */
1192  for(i=0;i<3;i++)
1193  {
1194  for(j=0;j<3;j++)
1195  HTransformSetElement(j,i,v[i][j],&(l->iR));
1196  }
1197  }
1198  }
1199 }
1200 
1201 unsigned int LinkNBodies(Tlink *l)
1202 {
1203  return(VectorSize(&(l->bodies)));
1204 }
1205 
1206 unsigned int LinkNAtoms(Tlink *l)
1207 {
1208  unsigned int i,m,n;
1209  Tpolyhedron *bd;
1210 
1211  /* Number of spheres used to represent atoms */
1212  n=0;
1213  m=LinkNBodies(l);
1214 
1215  if ((m>0)&&(VisibleLink(l)))
1216  {
1217  for(i=0;i<m;i++)
1218  {
1219  bd=GetLinkBody(i,l);
1220  if (GetPolyhedronType(bd)==SPHERE)
1221  n++;
1222  }
1223  }
1224  return(n);
1225 }
1226 
1227 Tpolyhedron *GetLinkBody(unsigned int i,Tlink *l)
1228 {
1229  Tpolyhedron **b;
1230 
1231  b=(Tpolyhedron **)GetVectorElement(i,&(l->bodies));
1232  if (b==NULL)
1233  return(NULL);
1234  else
1235  return(*b);
1236 }
1237 
1238 unsigned int GetLinkBodyStatus(unsigned int i,Tlink *l)
1239 {
1240  Tpolyhedron **b;
1241 
1242  b=(Tpolyhedron **)GetVectorElement(i,&(l->bodies));
1243  if (b==NULL)
1244  return(NO_UINT);
1245  else
1246  return(GetPolyhedronStatus(*b));
1247 }
1248 
1250 {
1251  return(l->name);
1252 }
1253 
1254 unsigned int GetLinkType(Tlink *l)
1255 {
1256  return(l->type);
1257 }
1258 
1260 {
1261  return(l->varLength);
1262 }
1263 
1264 unsigned int GetLinkForceModel(Tlink *l)
1265 {
1266  return(l->forceModel);
1267 }
1268 
1269 void GetLinkForceVars(unsigned int r,
1270  unsigned int *xID,unsigned int *yID,unsigned int *zID,
1271  unsigned int *le,
1272  unsigned int *re,unsigned int *f,
1273  TCuikSystem *cs,
1274  Tlink *l)
1275 {
1276  unsigned int vID[MAX_ROT_VARS];
1277 
1278  GetRotVarID(r,cs,vID,l);
1279  switch(l->forceModel)
1280  {
1281  case NO_FORCE:
1282  *xID=vID[0];
1283  *yID=vID[1];
1284  *zID=vID[2];
1285  *le=NO_UINT;
1286  *f=NO_UINT;
1287  *re=NO_UINT;
1288  break;
1289  case VAR_FORCE:
1290  *xID=vID[0];
1291  *yID=vID[1];
1292  *zID=vID[2];
1293  *le=vID[3];
1294  *f=vID[4];
1295  *re=NO_UINT;
1296  break;
1297  case LINEAR_FORCE:
1298  *xID=vID[0];
1299  *yID=vID[1];
1300  *zID=vID[2];
1301  *le=vID[3];
1302  *f=vID[4];
1303  *re=vID[5];
1304  break;
1305  default:
1306  Error("Unknown force model in GetLinkForcevars");
1307  }
1308 }
1309 
1310 double LinkPotentialEnergy(unsigned int r,TCuikSystem *cs,double *p,Tlink *l)
1311 {
1312  double e;
1313 
1314  /* Only consider elements with variable length. Element with constant length
1315  produce a constant offset to the energy level.
1316  Note that all variable length elements have possitive stiffness. */
1317  if ((l->varLength)&&(l->forceModel==LINEAR_FORCE))
1318  {
1319  double d;
1320  unsigned int vID[MAX_ROT_VARS];
1321 
1322  GetRotVarID(r,cs,vID,l);
1323 
1324  if ((vID[3]==NO_UINT)||(vID[5]==NO_UINT))
1325  Error("Missing force field variable");
1326 
1327  d=p[vID[3]]-p[vID[5]]; /* length - rest */
1328  e=0.5*l->stiffness*d*d; /* 0.5*k*(length-rest)^2 */
1329  }
1330  else
1331  e=0.0;
1332 
1333  return(e);
1334 }
1335 
1336 void LinkForceField(unsigned int r,TCuikSystem *cs,double *p,double *g,Tlink *l)
1337 {
1338  if ((l->varLength)&&(l->forceModel==LINEAR_FORCE))
1339  {
1340  double f;
1341  unsigned int vID[MAX_ROT_VARS];
1342 
1343  GetRotVarID(r,cs,vID,l);
1344 
1345  if ((vID[3]==NO_UINT)||(vID[5]==NO_UINT))
1346  Error("Missing force field variable");
1347 
1348  f=l->stiffness*(p[vID[3]]-p[vID[5]]); /* k*(length-rest)*/
1349 
1350  g[vID[3]]=+f;
1351  g[vID[5]]=-f;
1352  }
1353 }
1354 
1356 {
1357  InitTransSeq(ts);
1358 
1359  if (l->varLength)
1360  {
1361  char *vname;
1362  unsigned int vID;
1363 
1364  NEW(vname,strlen(l->name)+20,char);
1365 
1366  LINK_LENGTH(vname,l->name);
1367  vID=GetCSVariableID(vname,cs);
1368  if (vID==NO_UINT)
1369  Error("Undefined variable in GetLinkTransSeq");
1370  AddVarTrans2TransSeq(TX,1,vID,ts);
1371 
1372  free(vname);
1373  }
1374 }
1375 
1376 void GetLinkTransform(double *dof,THTransform *t,Tlink *l)
1377 {
1378  if (l->varLength)
1379  HTransformTx(dof[0],t);
1380  else
1381  HTransformIdentity(t);
1382 }
1383 
1385 {
1386  GetLinkTransform(def->p,t,l);
1387 }
1388 
1389 void GetLinkConfFromSolution(unsigned int r,TCuikSystem *cs,double *sol,TLinkConf *def,Tlink *l)
1390 {
1391  def->l=l;
1392  def->ndp=NumLinkDOF(l);
1393  if (def->ndp>0)
1394  {
1395  unsigned int k;
1396  unsigned int vID[MAX_ROT_VARS];
1397 
1398  GetRotVarID(r,cs,vID,l);
1399 
1400  NEW(def->p,def->ndp,double);
1401 
1402  k=0;
1403  if (l->varLength)
1404  {
1405  def->p[k]=sol[vID[3]]; /* length, if variable */
1406  k++;
1407  }
1408 
1409  if (l->forceModel!=NO_FORCE)
1410  {
1411  def->p[k]=sol[vID[4]]; /* force, if variable */
1412  k++;
1413  if (l->forceModel==LINEAR_FORCE)
1414  def->p[k]=sol[vID[5]]; /* rest, if variable */
1415  }
1416  }
1417  else
1418  def->p=NULL;
1419 }
1420 
1421 void GetLinkConfFromDOF(double *dof,TLinkConf *def,Tlink *l)
1422 {
1423  def->l=l;
1424  def->ndp=NumLinkDOF(l);
1425  if (def->ndp>0)
1426  {
1427  unsigned int k;
1428 
1429  NEW(def->p,def->ndp,double);
1430  k=0;
1431  if (l->varLength)
1432  {
1433  def->p[k]=dof[k]; /* length, if variable */
1434  k++;
1435  }
1436 
1437  if (l->forceModel!=NO_FORCE)
1438  {
1439  def->p[k]=dof[k]; /* force, if variable */
1440  k++;
1441  if (l->forceModel==LINEAR_FORCE)
1442  def->p[k]=dof[k]; /* rest, if variable */
1443  }
1444  }
1445  else
1446  def->p=NULL;
1447 }
1448 
1449 void GenerateSolutionFromLinkConf(unsigned int r,TCuikSystem *cs,TLinkConf *def,double *sol,Tlink *l)
1450 {
1451  if (def->l!=l)
1452  Error("Link missmatch in GenerateSolutionFromLinkConf");
1453  if (NumLinkDOF(l)>0)
1454  {
1455  unsigned int k;
1456  unsigned int vID[MAX_ROT_VARS];
1457 
1458  GetRotVarID(r,cs,vID,l);
1459 
1460  k=0;
1461  if (l->varLength)
1462  {
1463  sol[vID[3]]=def->p[k]; /* length, if variable */
1464  k++;
1465  }
1466 
1467  if (l->forceModel!=NO_FORCE)
1468  {
1469  sol[vID[4]]=def->p[k]; /* force, if variable */
1470  k++;
1471 
1472  if (l->forceModel==LINEAR_FORCE)
1473  sol[vID[5]]=def->p[k]; /* rest, if variable */
1474  }
1475  }
1476 }
1477 
1478 void GenerateDOFFromLinkConf(TLinkConf *def,double *dof,Tlink *l)
1479 {
1480  if (def->l!=l)
1481  Error("Link missmatch in GenerateDeformationDOF");
1482 
1483  if (def->ndp>0)
1484  memcpy(dof,def->p,sizeof(double)*def->ndp);
1485 }
1486 
1487 inline unsigned int NumLinkDOF(Tlink *l)
1488 {
1489  /*
1490  varLength -> length variable
1491  varForce -> force variable
1492  linearForce -> force variable + rest length
1493  */
1494  return((l->varLength?1:0)+(l->forceModel==VAR_FORCE?1:(l->forceModel==LINEAR_FORCE?2:0)));
1495 }
1496 
1497 Tinterval *GetLinkDOFRange(unsigned int i,Tlink *l)
1498 {
1499  unsigned int nd;
1500  Tinterval *s=NULL;
1501 
1502  nd=NumLinkDOF(l);
1503  if (nd>0)
1504  {
1505  if (i>=nd)
1506  Error("Wrong index in GetDeformationRange");
1507 
1508  switch(i)
1509  {
1510  case 0:
1511  if (l->varLength)
1512  s=&(l->length);
1513  else
1514  s=&(l->force);
1515  break;
1516  case 1:
1517  if (l->varLength)
1518  s=&(l->force);
1519  else
1520  s=&(l->rest);
1521  break;
1522  case 2:
1523  s=&(l->rest);
1524  }
1525  }
1526  else
1527  Error("Using GetDeformationRange on a link without internal degrees of freedom");
1528 
1529  return(s);
1530 }
1531 
1532 void GetLinkDOFLabel(unsigned int i,char **n,Tlink *l)
1533 {
1534  unsigned int nd;
1535 
1536  *n=NULL;
1537 
1538  nd=NumLinkDOF(l);
1539  if (nd>0)
1540  {
1541  if (i>=nd)
1542  Error("Wrong index in GetDeformationLabel");
1543 
1544  NEW(*n,20,char);
1545  switch(i)
1546  {
1547  case 0:
1548  if (l->varLength)
1549  sprintf(*n,"Elongation");
1550  else
1551  sprintf(*n,"Force");
1552  break;
1553  case 1:
1554  if (l->varLength)
1555  sprintf(*n,"Force");
1556  else
1557  sprintf(*n,"Rest");
1558  break;
1559  case 2:
1560  sprintf(*n,"Rest");
1561  }
1562  }
1563  else
1564  Error("Using GetDeformationLabel on a non-deformable link");
1565 }
1566 
1568 {
1569  return(l->allSpheres);
1570 }
1571 
1572 void SetPoseVars(unsigned int r,boolean *vars,TCuikSystem *cs,Tlink *l)
1573 {
1574  SetTransVars(vars,cs,l);
1575  SetRotVars(r,vars,cs,l);
1576 }
1577 
1578 unsigned int SetForceVars(unsigned int r,boolean *vars,TCuikSystem *cs,Tlink *l)
1579 {
1580  unsigned int n;
1581 
1582  if (l->forceModel!=NO_FORCE)
1583  {
1584  unsigned int vID[MAX_ROT_VARS];
1585 
1586  GetRotVarID(r,cs,vID,l);
1587  vars[vID[4]]=TRUE; /* force */
1588  n=1;
1589  }
1590  else
1591  n=0;
1592 
1593  return(n);
1594 }
1595 
1596 unsigned int SetForceRelatedVars(unsigned int r,boolean *vars,TCuikSystem *cs,Tlink *l)
1597 {
1598  unsigned int n;
1599 
1600  if (l->forceModel!=NO_FORCE)
1601  {
1602  unsigned int vID[MAX_ROT_VARS];
1603 
1604  GetRotVarID(r,cs,vID,l);
1605  vars[vID[4]]=TRUE; /* force */
1606  n=1;
1607  if (l->forceModel==LINEAR_FORCE)
1608  {
1609  vars[vID[5]]=TRUE; /* rest length */
1610  n++;
1611  }
1612  }
1613  else
1614  n=0;
1615 
1616  return(n);
1617 }
1618 
1619 void SetTransVars(boolean *vars,TCuikSystem *cs,Tlink *l)
1620 {
1621  char *vname;
1622  unsigned int k,id;
1623 
1624  NEW(vname,strlen(l->name)+100,char);
1625  for(k=0;k<3;k++)
1626  {
1627  LINK_TRANS(vname,l->name,k);
1628  id=GetCSVariableID(vname,cs);
1629  if (id!=NO_UINT) /*Ground links has no variables*/
1630  vars[id]=TRUE;
1631  }
1632  free(vname);
1633 }
1634 
1635 void GetRotVarID(unsigned int r,TCuikSystem *cs,unsigned int *vID,Tlink *l)
1636 {
1637  /* Many threads can access the link information in parallel. We must be sure
1638  that the information stored in the link itself is valid and consistent
1639  (i.e., that no other thread re-writes it while we access it).
1640  In some particular condition we might have a race issue here... but not
1641  in the way we use this function (csvID is only changed once from NULL
1642  to a particular value. We do not call this function with different cs's)*/
1643  if (cs!=l->csvID)
1644  {
1645  #pragma omp critical
1646  {
1647  /* The first thread that enters the critical section changes l->csvID
1648  The threads possibly waiting in the critical section can skip the
1649  update.
1650  Note that if a process is inside the critical region csvID is NULL
1651  and, thus the 'if' above will not hold during this period, i.e., when
1652  a process is modifying the cache, all other process will wait at the
1653  beginning of the critial region. When they cross it, they might
1654  eventually realize that the work is already done*/
1655  if (cs!=l->csvID)
1656  {
1657  l->csvID=NULL; /* to force threads to wait in the critial region */
1658 
1659  /* When we are in this block is for sure that we are the only
1660  thread manipulating the link information. */
1661  /* The information stored in the link is not valid for 'cs'.
1662  Re-generate the link information. In general, the information
1663  is only generated once and later calls to this function just
1664  use the memcpy below -> they are fast. */
1665  switch(l->type)
1666  {
1667  case LINK_AxisX:
1668  case LINK_DeformX:
1669  case LINK_PrismaticX:
1670  /* Those links have relevant variables (force-related) even
1671  if representation is JOINTS */
1672  GetRotVarIDAxisX(cs,l);
1673  break;
1674  case LINK_FullRot:
1675  switch(r)
1676  {
1677  case REP_LINKS:
1678  GetRotVarIDLinks(cs,l);
1679  break;
1680  case REP_FLINKS:
1681  GetRotVarIDFLinks(cs,l);
1682  break;
1683  case REP_QLINKS:
1684  GetRotVarIDQLinks(cs,l);
1685  break;
1686  case REP_JOINTS:
1687  /* Full Rot links generate no joint variable */
1688  break;
1689  default:
1690  Error("Undefined representation type in GetRotVarID");
1691  }
1692  break;
1693  case LINK_NoRot:
1694  /* no variable is used for NoRot links */
1695  break;
1696  default:
1697  Error("Unkown link type in GetRotVarID");
1698  }
1699  l->csvID=cs; /* from this point on the cache is valid and can be accesed in parallel */
1700  }
1701  }
1702  }
1703 
1704  /* Copy the information stored in the link. */
1705  memcpy(vID,l->vID,sizeof(unsigned int)*l->nvID);
1706 }
1707 
1709 {
1710  char *vname;
1711  unsigned int i;
1712 
1713  l->nvID=6;
1714  if (l->csvID==NULL)
1715  NEW(l->vID,l->nvID,unsigned int);
1716 
1717  NEW(vname,strlen(l->name)+100,char);
1718  for(i=0;i<3;i++)
1719  {
1720  LINK_ROT(vname,l->name,0,i);
1721  l->vID[i]=GetCSVariableID(vname,cs);
1722  }
1723 
1724  if (l->varLength)
1725  {
1726  LINK_LENGTH(vname,l->name);
1727  l->vID[3]=GetCSVariableID(vname,cs);
1728  }
1729  else
1730  l->vID[3]=NO_UINT;
1731 
1732  /* now the compression/tension parameter, if any */
1733  if (l->forceModel!=NO_FORCE)
1734  {
1735  LINK_MAX_FORCE(vname,l->name);
1736  l->vID[4]=GetCSVariableID(vname,cs);
1737 
1738  if (l->forceModel==LINEAR_FORCE)
1739  {
1740  LINK_REST(vname,l->name);
1741  l->vID[5]=GetCSVariableID(vname,cs);
1742  }
1743  else
1744  l->vID[5]=NO_UINT;
1745  }
1746  else
1747  l->vID[4]=l->vID[5]=NO_UINT;
1748 
1749  free(vname);
1750 }
1751 
1753 {
1754  char *vname;
1755  unsigned int i,j,k;
1756 
1757  l->nvID=9;
1758  if (l->csvID==NULL)
1759  NEW(l->vID,l->nvID,unsigned int);
1760 
1761  NEW(vname,strlen(l->name)+100,char);
1762  k=0;
1763  for(i=0;i<3;i++) /*vector*/
1764  {
1765  for(j=0;j<3;j++) /*component*/
1766  {
1767  LINK_ROT(vname,l->name,i,j);
1768  /* for the ground link all vID will be NO_UINT. Use at your own
1769  risk. */
1770  l->vID[k]=GetCSVariableID(vname,cs);
1771  k++;
1772  }
1773  }
1774  free(vname);
1775 }
1776 
1778 {
1779  char *vname;
1780  unsigned int i,j,k;
1781 
1782  l->nvID=12;
1783  if (l->csvID==NULL)
1784  NEW(l->vID,l->nvID,unsigned int);
1785 
1786  NEW(vname,strlen(l->name)+100,char);
1787  k=0;
1788  for(i=0;i<4;i++) /*vector*/
1789  {
1790  for(j=0;j<3;j++) /*component*/
1791  {
1792  LINK_ROT2(vname,l->name,i,j);
1793  /* for the ground link all vID will be NO_UINT. Use at your own*/
1794  l->vID[k]=GetCSVariableID(vname,cs);
1795  k++;
1796  }
1797  }
1798  free(vname);
1799 }
1800 
1802 {
1803  char *vname;
1804  unsigned int i,j,k;
1805 
1806  NEW(vname,strlen(l->name)+100,char);
1807 
1808  l->nvID=14;
1809  if (l->csvID==NULL)
1810  NEW(l->vID,l->nvID,unsigned int);
1811 
1812  k=0;
1813  for(j=0;j<4;j++)
1814  {
1815  LINK_ROT3_Q(vname,l->name,j);
1816  l->vID[k]=GetCSVariableID(vname,cs);
1817  k++;
1818  }
1819 
1820  for(i=0;i<4;i++)
1821  {
1822  for(j=i;j<4;j++)
1823  {
1824  LINK_ROT3_E(vname,l->name,i,j);
1825  /* for the ground link all vID will be NO_UINT. Use at your own risk */
1826  l->vID[k]=GetCSVariableID(vname,cs);
1827  k++;
1828  }
1829  }
1830 
1831  free(vname);
1832 }
1833 
1834 void SetRotVars(unsigned int r,boolean *vars,TCuikSystem *cs,Tlink *l)
1835 {
1836  if (r!=REP_JOINTS) /* When using joint representation links
1837  do not generate variables */
1838  {
1839  unsigned int i,id,n=0;
1840  unsigned int vID[MAX_ROT_VARS];
1841 
1842  GetRotVarID(r,cs,vID,l);
1843  switch(l->type)
1844  {
1845  case LINK_AxisX:
1846  case LINK_DeformX:
1847  case LINK_PrismaticX:
1848  n=6; /* max 6 system vars: 3 director vector + 3 length/force/rest */
1849  break;
1850  case LINK_FullRot:
1851  switch(r)
1852  {
1853  case REP_LINKS:
1854  n=6;
1855  break;
1856  case REP_FLINKS:
1857  n=9;
1858  break;
1859  case REP_QLINKS:
1860  n=4;
1861  break;
1862  default:
1863  Error("Undefined representation type in SetRotVars");
1864  }
1865  break;
1866  case LINK_NoRot:
1867  n=0;
1868  break;
1869  default:
1870  Error("Unknown link type in SetRotVars");
1871  }
1872 
1873  for(i=0;i<n;i++)
1874  {
1875  id=vID[i];
1876  if (id!=NO_UINT) /*Ground links has not variables*/
1877  vars[id]=TRUE;
1878  }
1879 
1880  }
1881 }
1882 
1883 void GetLinkPoseSimpVars(Tparameters *p,boolean *sv,
1884  TCuikSystem *cs,Tlink *l)
1885 {
1886  unsigned int r;
1887 
1888  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
1889  if (r!=REP_JOINTS) /* When using joint representation links
1890  do not generate variables */
1891  {
1892  unsigned int i,id,n=0;
1893  char *v;
1894  unsigned int vID[MAX_ROT_VARS];
1895 
1896  GetRotVarID(r,cs,vID,l);
1897 
1898  switch(l->type)
1899  {
1900  case LINK_AxisX:
1901  case LINK_DeformX:
1902  case LINK_PrismaticX:
1903  n=6; /* max 6 system vars: 3 director vector + 3 length/force/rest */
1904  break;
1905  case LINK_FullRot:
1906  switch(r)
1907  {
1908  case REP_LINKS:
1909  n=6;
1910  break;
1911  case REP_FLINKS:
1912  n=9;
1913  break;
1914  case REP_QLINKS:
1915  n=4;
1916  break;
1917  default:
1918  Error("Undefined representation type in SetRotVars");
1919  }
1920  break;
1921  case LINK_NoRot:
1922  n=0;
1923  break;
1924  default:
1925  Error("Unknown link type in SetRotVars");
1926  }
1927 
1928  for(i=0;i<n;i++)
1929  {
1930  id=vID[i];
1931  if (id!=NO_UINT) /*Ground links has not variables*/
1932  {
1933  v=GetCSVariableName(id,cs);
1934  if (IsSystemVarInSimpCS(p,v,cs))
1935  sv[id]=TRUE;
1936  }
1937  }
1938  }
1939 }
1940 
1941 void GenerateLinkRot(Tparameters *p,unsigned int lID,
1942  TCuikSystem *cs,Tlink *l)
1943 {
1944  unsigned int r;
1945 
1946  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
1947  if (r!=REP_JOINTS)
1948  {
1949  switch(l->type)
1950  {
1951  case LINK_AxisX:
1952  case LINK_DeformX:
1953  case LINK_PrismaticX:
1954  GenerateLinkRotAxisX(p,lID,cs,l);
1955  break;
1956  case LINK_FullRot:
1957  switch(r)
1958  {
1959  case REP_LINKS:
1960  GenerateLinkRotLinks(p,lID,cs,l);
1961  break;
1962  case REP_FLINKS:
1963  GenerateLinkRotFLinks(p,lID,cs,l);
1964  break;
1965  case REP_QLINKS:
1966  GenerateLinkRotQLinks(p,lID,cs,l);
1967  break;
1968  default:
1969  Error("Undefined representation type in GenerateLinkRot");
1970  }
1971  break;
1972  case LINK_NoRot:
1973  break;
1974  default:
1975  Error("Unkonw link type in GenerateLinkRot");
1976  }
1977  }
1978  GenerateLinkConf(p,cs,l);
1979 }
1980 
1982 {
1983  unsigned int le,re,fr,rep;
1984  char *vname;
1985  Tvariable var;
1986 
1987  NEW(vname,strlen(l->name)+100,char);
1988 
1989  if (l->varLength)
1990  {
1991  LINK_LENGTH(vname,l->name);
1992  /* Check that the var/equation are not already created */
1993  if (GetCSVariableID(vname,cs)==NO_UINT)
1994  {
1995  NewVariable(SYSTEM_VAR,vname,&var);
1996  SetVariableInterval(&(l->length),&var);
1997  le=AddVariable2CS(&var,cs);
1998  DeleteVariable(&var);
1999  }
2000  }
2001  else
2002  le=NO_UINT;
2003 
2004  if (l->forceModel!=NO_FORCE)
2005  {
2006  /* We generate the variables even if they have
2007  ct range */
2008  LINK_MAX_FORCE(vname,l->name);
2009 
2010  /* Check that the force var/equation are not already created */
2011  if (GetCSVariableID(vname,cs)==NO_UINT)
2012  {
2013  Tmonomial f;
2014  Tequation eq;
2015 
2016  if (l->forceModel==LINEAR_FORCE)
2017  {
2018  /* Create the rest length variable */
2019  LINK_REST(vname,l->name);
2020  NewVariable(SYSTEM_VAR,vname,&var);
2021  SetVariableInterval(&(l->rest),&var);
2022  re=AddVariable2CS(&var,cs);
2023  DeleteVariable(&var);
2024  }
2025 
2026  /* actually create the force variable */
2027  LINK_MAX_FORCE(vname,l->name);
2028  NewVariable(SYSTEM_VAR,vname,&var);
2029  SetVariableInterval(&(l->force),&var);
2030  fr=AddVariable2CS(&var,cs);
2031  DeleteVariable(&var);
2032 
2033  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2034 
2035  if (((rep==REP_LINKS)||(rep==REP_FLINKS))&&(l->forceModel==LINEAR_FORCE))
2036  {
2037  /* Now the equation linking length, stiffnes, rest and force. */
2038  InitEquation(&eq);
2039  InitMonomial(&f);
2040 
2041  /* stiffness * length */
2042  AddCt2Monomial(l->stiffness,&f);
2043  if (le==NO_UINT)
2044  Error("Can not have constant length in linear force models");
2045  else
2046  AddVariable2Monomial(NFUN,le,1,&f);
2047  AddMonomial(&f,&eq);
2048  ResetMonomial(&f);
2049 
2050  /* -stiffness * rest_length */
2051  AddCt2Monomial(-l->stiffness,&f);
2052  AddVariable2Monomial(NFUN,re,1,&f);
2053  AddMonomial(&f,&eq);
2054  ResetMonomial(&f);
2055 
2056  AddCt2Monomial(-1,&f); /* = force */
2057  AddVariable2Monomial(NFUN,fr,1,&f);
2058  AddMonomial(&f,&eq);
2059  DeleteMonomial(&f);
2060 
2061  SetEquationCmp(EQU,&eq);
2062  SetEquationValue(0,&eq);
2064 
2065  AddEquation2CS(p,&eq,cs);
2066  DeleteEquation(&eq);
2067  }
2068  }
2069  }
2070  free(vname);
2071 }
2072 
2073 void GenerateLinkRotAxisX(Tparameters *p,unsigned int lID,TCuikSystem *cs,Tlink *l)
2074 {
2075  if (!IsGroundLink(lID))
2076  {
2077  char *vname;
2078 
2079  NEW(vname,strlen(l->name)+100,char);
2080  LINK_ROT(vname,l->name,0,0);
2081 
2082  /* only create the director vector if it is not already there */
2083  if (GetCSVariableID(vname,cs)==NO_UINT)
2084  {
2085  unsigned int j;
2086  Tinterval range;
2087  Tequation eqn;
2088  unsigned int linkVars[3];
2089  Tvariable var;
2090 
2091  NewInterval(-1.0,1.0,&range);
2092  for(j=0;j<3;j++) /*component*/
2093  {
2094  LINK_ROT(vname,l->name,0,j);
2095  NewVariable(SYSTEM_VAR,vname,&var);
2096  SetVariableInterval(&range,&var);
2097  linkVars[j]=AddVariable2CS(&var,cs);
2098  DeleteVariable(&var);
2099  }
2100 
2101  GenerateNormEquation(linkVars[0],linkVars[1],linkVars[2],1.0,&eqn);
2102  AddEquation2CS(p,&eqn,cs);
2103  DeleteEquation(&eqn);
2104  }
2105  free(vname);
2106  }
2107 }
2108 
2109 void GenerateLinkRotFLinks(Tparameters *p,unsigned int lID,TCuikSystem *cs,Tlink *l)
2110 {
2111  if (!IsGroundLink(lID))
2112  {
2113  char *vname;
2114 
2115  /*If the cuiksytem already has the link variables (and thus the corresponding equations)
2116  there is nothing to be done. */
2117 
2118  NEW(vname,strlen(l->name)+100,char);
2119 
2120  LINK_ROT(vname,l->name,0,0); /* This variable is used both in LINK_FullRot and LINK_AxisX */
2121 
2122  if (GetCSVariableID(vname,cs)==NO_UINT)
2123  {
2124  unsigned int i,j;
2125  Tinterval range;
2126  Tequation eqn;
2127  Tequation eq[3];
2128  unsigned int linkVars[9];
2129  Tvariable var;
2130 
2131  NewInterval(-1.0,1.0,&range);
2132  for(i=0;i<3;i++) /*vector*/
2133  {
2134  for(j=0;j<3;j++) /*component*/
2135  {
2136  LINK_ROT(vname,l->name,i,j);
2137  NewVariable(SYSTEM_VAR,vname,&var);
2138  SetVariableInterval(&range,&var);
2139  linkVars[i*3+j]=AddVariable2CS(&var,cs);
2140  DeleteVariable(&var);
2141  }
2142  }
2143 
2144  for(i=0;i<2+ROT_REDUNDANCY;i++)
2145  {
2146  GenerateNormEquation(linkVars[3*i+0],linkVars[3*i+1],linkVars[3*i+2],1.0,&eqn);
2147  AddEquation2CS(p,&eqn,cs);
2148  DeleteEquation(&eqn);
2149  }
2150 
2151  /*ONLY ONE OF THE THREE FOLLOWING BLOCKS IS REQUIRED, THE OTHER TWO ARE REDUNDANT. */
2152  #if (ROT_REDUNDANCY==1)
2153  /*********************************************************************/
2154 
2155  GenerateDotProductEquation(linkVars[0],linkVars[1],linkVars[2], /* X */
2156  linkVars[3],linkVars[4],linkVars[5], /* Y */
2157  NO_UINT,l->c,&eqn);
2158  AddEquation2CS(p,&eqn,cs);
2159  DeleteEquation(&eqn);
2160 
2161 
2162  GenerateCrossProductEquations(linkVars[0],linkVars[1],linkVars[2], /* X */
2163  linkVars[3],linkVars[4],linkVars[5], /* Y */
2164  linkVars[6],linkVars[7],linkVars[8], /* Z */
2165  NO_UINT,l->s,eq);
2166  for(i=0;i<3;i++)
2167  {
2168  AddEquation2CS(p,&(eq[i]),cs);
2169  DeleteEquation(&(eq[i]));
2170  }
2171  /*********************************************************************/
2172  #endif
2173 
2174  /*********************************************************************/
2175  GenerateDotProductEquation(linkVars[6],linkVars[7],linkVars[8], /* Z */
2176  linkVars[0],linkVars[1],linkVars[2], /* X */
2177  NO_UINT,0,&eqn);
2178  AddEquation2CS(p,&eqn,cs);
2179  DeleteEquation(&eqn);
2180  if (l->s==1) /*This second redundant equation is only used for orthogonal X-Y vectors */
2181  {
2182  GenerateCrossProductEquations(linkVars[6],linkVars[7],linkVars[8], /* Z */
2183  linkVars[0],linkVars[1],linkVars[2], /* X */
2184  linkVars[3],linkVars[4],linkVars[5], /* Y */
2185  NO_UINT,1.0,eq);
2186  for(i=0;i<3;i++)
2187  {
2188  AddEquation2CS(p,&(eq[i]),cs);
2189  DeleteEquation(&(eq[i]));
2190  }
2191  }
2192  /*********************************************************************/
2193 
2194  #if (ROT_REDUNDANCY==1) /*Redundancy is only used for orthogonal X-Y vectors */
2195  /*********************************************************************/
2196  GenerateDotProductEquation(linkVars[3],linkVars[4],linkVars[5], /* Y */
2197  linkVars[6],linkVars[7],linkVars[8], /* Z */
2198  NO_UINT,0,&eqn);
2199  AddEquation2CS(p,&eqn,cs);
2200  DeleteEquation(&eqn);
2201 
2202 
2203  if (l->s==1) /*This second redundant equation is only used for orthogonal X-Y vectors */
2204  {
2205  GenerateCrossProductEquations(linkVars[3],linkVars[4],linkVars[5], /* Y */
2206  linkVars[6],linkVars[7],linkVars[8], /* Z */
2207  linkVars[0],linkVars[1],linkVars[2], /* X */
2208  NO_UINT,1.0,eq);
2209  for(i=0;i<3;i++)
2210  {
2211  AddEquation2CS(p,&(eq[i]),cs);
2212  DeleteEquation(&(eq[i]));
2213  }
2214  }
2215  /*********************************************************************/
2216  #endif
2217  }
2218 
2219  free(vname);
2220  }
2221 }
2222 
2223 void GenerateLinkRotLinks(Tparameters *p,unsigned int lID,TCuikSystem *cs,Tlink *l)
2224 {
2225  if (!IsGroundLink(lID))
2226  {
2227  char *vname;
2228 
2229  /*If the cuiksytem already has the link variables (and thus the corresponding equations)
2230  there is nothing to be done. */
2231 
2232  NEW(vname,strlen(l->name)+100,char);
2233 
2234  LINK_ROT(vname,l->name,0,0);
2235 
2236  if (GetCSVariableID(vname,cs)==NO_UINT)
2237  {
2238  unsigned int i,j;
2239  Tinterval range;
2240  Tequation eqn;
2241  unsigned int linkVars[12];
2242  Tvariable var;
2243 
2244  NewInterval(-1.0,1.0,&range);
2245  /* vectors u,v,w,wp */
2246  for(i=0;i<4;i++) /*vector*/
2247  {
2248  for(j=0;j<3;j++) /*component*/
2249  {
2250  LINK_ROT2(vname,l->name,i,j);
2251  NewVariable((i<2?SYSTEM_VAR:DUMMY_VAR),vname,&var);
2252  SetVariableInterval(&range,&var);
2253  linkVars[i*3+j]=AddVariable2CS(&var,cs);
2254  DeleteVariable(&var);
2255  }
2256  }
2257 
2258  /* define w and wp from x and y */
2259  GenerateScaledSaddleEquation(l->s,linkVars[1],linkVars[5],linkVars[6],&eqn);
2260  AddEquation2CS(p,&eqn,cs);
2261  DeleteEquation(&eqn);
2262 
2263  GenerateScaledSaddleEquation(l->s,linkVars[2],linkVars[3],linkVars[7],&eqn);
2264  AddEquation2CS(p,&eqn,cs);
2265  DeleteEquation(&eqn);
2266 
2267  GenerateScaledSaddleEquation(l->s,linkVars[0],linkVars[4],linkVars[8],&eqn);
2268  AddEquation2CS(p,&eqn,cs);
2269  DeleteEquation(&eqn);
2270 
2271  GenerateScaledSaddleEquation(l->s,linkVars[2],linkVars[4],linkVars[9],&eqn);
2272  AddEquation2CS(p,&eqn,cs);
2273  DeleteEquation(&eqn);
2274 
2275  GenerateScaledSaddleEquation(l->s,linkVars[0],linkVars[5],linkVars[10],&eqn);
2276  AddEquation2CS(p,&eqn,cs);
2277  DeleteEquation(&eqn);
2278 
2279  GenerateScaledSaddleEquation(l->s,linkVars[1],linkVars[3],linkVars[11],&eqn);
2280  AddEquation2CS(p,&eqn,cs);
2281  DeleteEquation(&eqn);
2282 
2283 
2284  GenerateDotProductEquation(linkVars[0],linkVars[1],linkVars[2], /* X */
2285  linkVars[3],linkVars[4],linkVars[5], /* Y */
2286  NO_UINT,l->c,&eqn);
2287  AddEquation2CS(p,&eqn,cs);
2288  DeleteEquation(&eqn);
2289 
2290  /* norm of x and y */
2291  for(i=0;i<2;i++)
2292  {
2293  GenerateNormEquation(linkVars[3*i+0],linkVars[3*i+1],linkVars[3*i+2],1.0,&eqn);
2294  AddEquation2CS(p,&eqn,cs);
2295  DeleteEquation(&eqn);
2296  }
2297  }
2298 
2299  free(vname);
2300  }
2301 }
2302 
2303 void GenerateLinkRotQLinks(Tparameters *p,unsigned int lID,TCuikSystem *cs,Tlink *l)
2304 {
2305  if (!IsGroundLink(lID))
2306  {
2307  char *vname;
2308 
2309  /*If the cuiksytem already has the link variables (and thus the corresponding equations)
2310  there is nothing to be done. */
2311 
2312  NEW(vname,strlen(l->name)+100,char);
2313 
2314  LINK_ROT3_Q(vname,l->name,0); /* used both in LINK_FullRot and LINK_AxisX */
2315 
2316  if (GetCSVariableID(vname,cs)==NO_UINT)
2317  {
2318  unsigned int i,j,n;
2319  Tinterval range,range2;
2320  Tequation eqn;
2321  unsigned int linkVars[10];
2322  unsigned int qVars[4];
2323  Tvariable var;
2324  Tmonomial f;
2325 
2326  NewInterval(-1.0,1.0,&range);
2327  NewInterval(0.0,1.0,&range2);
2328 
2329  /* q variables are used in all cases */
2330  for(j=0;j<4;j++)
2331  {
2332  LINK_ROT3_Q(vname,l->name,j);
2333  NewVariable(SYSTEM_VAR,vname,&var);
2334  if (j==0)
2335  SetVariableInterval(&range2,&var);
2336  else
2337  SetVariableInterval(&range,&var);
2338  qVars[j]=AddVariable2CS(&var,cs);
2339  DeleteVariable(&var);
2340  }
2341 
2342  /* Add the relation between the E variables (dummies) and the
2343  Q variables (system) */
2344 
2345  n=0;
2346  for(i=0;i<4;i++)
2347  {
2348  for(j=i;j<4;j++)
2349  {
2350  LINK_ROT3_E(vname,l->name,i,j);
2351  NewVariable(DUMMY_VAR,vname,&var);
2352  if (i==j)
2353  SetVariableInterval(&range2,&var);
2354  else
2355  SetVariableInterval(&range,&var);
2356  linkVars[n]=AddVariable2CS(&var,cs);
2357  n++;
2358  DeleteVariable(&var);
2359  }
2360  }
2361 
2362  n=0;
2363  for(i=0;i<4;i++)
2364  {
2365  for(j=i;j<4;j++)
2366  {
2367  /* When i=j (-> qVars[i]==qVars[j]) a parabola equation is generated! */
2368  GenerateSaddleEquation(qVars[i],qVars[j],linkVars[n],&eqn);
2369  AddEquation2CS(p,&eqn,cs);
2370  DeleteEquation(&eqn);
2371  n++;
2372  }
2373  }
2374 
2375  /* norm of the Q (system) variables is 1 (we use the already introduced
2376  dummy e_i variables) */
2377  InitEquation(&eqn);
2378  InitMonomial(&f);
2379 
2380  AddVariable2Monomial(NFUN,linkVars[0],1,&f);
2381  AddMonomial(&f,&eqn);ResetMonomial(&f);
2382  AddVariable2Monomial(NFUN,linkVars[4],1,&f);
2383  AddMonomial(&f,&eqn);ResetMonomial(&f);
2384  AddVariable2Monomial(NFUN,linkVars[7],1,&f);
2385  AddMonomial(&f,&eqn);ResetMonomial(&f);
2386  AddVariable2Monomial(NFUN,linkVars[9],1,&f);
2387  AddMonomial(&f,&eqn);
2388 
2389 
2390  /* add the q normalization equation */
2391  DeleteMonomial(&f);
2392 
2393  SetEquationCmp(EQU,&eqn);
2394  SetEquationValue(1,&eqn);
2395  SetEquationType(SYSTEM_EQ,&eqn);
2396 
2397  AddEquation2CS(p,&eqn,cs);
2398  DeleteEquation(&eqn);
2399 
2400  }
2401  free(vname);
2402  }
2403 }
2404 
2405 void FixLinkZToZero(Tparameters *p,unsigned int lID,TCuikSystem *cs,Tlink *l)
2406 {
2407  unsigned int r;
2408 
2409  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2410  if (r!=REP_JOINTS)
2411  {
2412  if ((l->type==LINK_AxisX)||(l->type==LINK_DeformX)||(l->type==LINK_PrismaticX))
2413  {
2414  Tmonomial f;
2415  Tequation eq;
2416  unsigned int vID[MAX_ROT_VARS];
2417 
2418  if (IsGroundLink(lID))
2419  Error("The root link of a tensegrity can not be an active element");
2420 
2421  GetRotVarID(r,cs,vID,l);
2422 
2423  /* z */
2424  InitEquation(&eq);
2425  InitMonomial(&f);
2426  AddVariable2Monomial(NFUN,vID[2],1,&f);
2427  AddMonomial(&f,&eq);
2428  DeleteMonomial(&f);
2429 
2430  /* = 0 */
2431  SetEquationCmp(EQU,&eq);
2432  SetEquationValue(0.0,&eq);
2433 
2435  AddEquation2CS(p,&eq,cs);
2436  DeleteEquation(&eq);
2437  }
2438  }
2439 }
2440 
2442  double sf,unsigned int sv,double *p,Tequation *eq,
2443  TCuikSystem *cs,boolean groundLink,Tlink *l)
2444 {
2445  unsigned int r;
2446 
2447  r=(unsigned int)(GetParameter(CT_REPRESENTATION,pr));
2448  if ((r!=REP_JOINTS)&&(fabs(sf)*Norm(3,p)>ZERO))
2449  {
2450  switch(l->type)
2451  {
2452  case LINK_AxisX:
2453  ApplyLinkRotAxisX(sf,sv,p,eq,cs,groundLink,l);
2454  break;
2455  case LINK_DeformX:
2456  case LINK_PrismaticX:
2457  ApplyLinkRotDeformX(pr,sf,sv,p,eq,cs,groundLink,l);
2458  break;
2459  case LINK_FullRot:
2460  switch(r)
2461  {
2462  case REP_LINKS:
2463  ApplyLinkRotLinks(sf,sv,p,eq,cs,groundLink,l);
2464  break;
2465  case REP_FLINKS:
2466  ApplyLinkRotFLinks(sf,sv,p,eq,cs,groundLink,l);
2467  break;
2468  case REP_QLINKS:
2469  ApplyLinkRotQLinks(sf,sv,p,eq,cs,groundLink,l);
2470  break;
2471  default:
2472  Error("Undefined representation type in ApplyLinkRot");
2473  }
2474  break;
2475  case LINK_NoRot:
2476  /* NoRot links have the same orientation as the global frame */
2477  switch(r)
2478  {
2479  case REP_LINKS:
2480  ApplyLinkRotLinks(sf,sv,p,eq,cs,TRUE,l);
2481  break;
2482  case REP_FLINKS:
2483  ApplyLinkRotFLinks(sf,sv,p,eq,cs,TRUE,l);
2484  break;
2485  case REP_QLINKS:
2486  ApplyLinkRotQLinks(sf,sv,p,eq,cs,TRUE,l);
2487  break;
2488  default:
2489  Error("Undefined representation type in ApplyLinkRot");
2490  }
2491  break;
2492  default:
2493  Error("Unkown link type in ApplyLinkRot");
2494  }
2495  }
2496 }
2497 
2499  double sf,unsigned int sv,double *p,Tequation *eq,
2500  TCuikSystem *cs,boolean groundLink,Tlink *l)
2501 {
2502  if (l->varLength)
2503  ApplyLinkRotAxisX(sf,sv,p,eq,cs,groundLink,l);
2504  else
2505  ApplyLinkRot(pr,sf,sv,p,eq,cs,groundLink,l);
2506 }
2507 
2508 void ApplyLinkRotAxisX(double sf,unsigned int sv,double *p,Tequation *eq,
2509  TCuikSystem *cs,boolean groundLink,Tlink *l)
2510 {
2511  Tmonomial f;
2512 
2513  /*
2514  sf*sv*T(px 0 0) where T is defined from the linkVars (check that hasVars==TRUE)
2515  T only has x component
2516  */
2517  if (Norm(2,&(p[1]))>ZERO)
2518  Error("X only links can only be used on points on the X-axis");
2519 
2520  InitMonomial(&f);
2521  if (groundLink)
2522  {
2523  if (sv!=NO_UINT)
2524  AddVariable2Monomial(NFUN,sv,1,&f);
2525  AddCt2Monomial(sf*p[0],&f);
2526 
2527  AddMonomial(&f,&(eq[0]));
2528  ResetMonomial(&f);
2529  }
2530  else
2531  {
2532  unsigned int j;
2533  unsigned int linkVars[3];
2534  char *vname;
2535 
2536  NEW(vname,strlen(l->name)+100,char);
2537 
2538  for(j=0;j<3;j++)
2539  {
2540  LINK_ROT(vname,l->name,0,j);
2541  linkVars[j]=GetCSVariableID(vname,cs);
2542  if (linkVars[j]==NO_UINT)
2543  Error("Undefined reference variable in ApplyLinkRotAxisX");
2544  }
2545 
2546  for(j=0;j<3;j++)
2547  {
2548  AddVariable2Monomial(NFUN,linkVars[j],1,&f);
2549  if (sv!=NO_UINT)
2550  AddVariable2Monomial(NFUN,sv,1,&f);
2551  AddCt2Monomial(sf*p[0],&f);
2552 
2553  AddMonomial(&f,&(eq[j]));
2554  ResetMonomial(&f);
2555  }
2556 
2557  free(vname);
2558  }
2559  DeleteMonomial(&f);
2560 }
2561 
2562 void ApplyLinkRotDeformX(Tparameters *pr,double sf,unsigned int sv,double *p,Tequation *eq,
2563  TCuikSystem *cs,boolean groundLink,Tlink *l)
2564 {
2565  Tmonomial f;
2566  unsigned int j;
2567  unsigned int linkVars[4];
2568  char *vname;
2569 
2570  /*
2571  sf*sv*T(px 0 0) where T is defined from the linkVars (check that hasVars==TRUE)
2572  T only has x component
2573  */
2574  if (Norm(2,&(p[1]))>ZERO)
2575  Error("Deform X only links can only be used on points on the X-axis");
2576 
2577  NEW(vname,strlen(l->name)+100,char);
2578  LINK_LENGTH(vname,l->name);
2579  linkVars[3]=GetCSVariableID(vname,cs);
2580  if (linkVars[3]==NO_UINT)
2581  Error("Undefined reference variable in ApplyLinkRotDeformX");
2582 
2583  InitMonomial(&f);
2584  if (groundLink)
2585  {
2586  if (sv!=NO_UINT)
2587  AddVariable2Monomial(NFUN,sv,1,&f);
2588  AddVariable2Monomial(NFUN,linkVars[3],1,&f);
2589  AddCt2Monomial(sf*p[0],&f);
2590 
2591  AddMonomial(&f,&(eq[0]));
2592  ResetMonomial(&f);
2593  }
2594  else
2595  {
2596  for(j=0;j<3;j++)
2597  {
2598  LINK_ROT(vname,l->name,0,j);
2599  linkVars[j]=GetCSVariableID(vname,cs);
2600  if (linkVars[j]==NO_UINT)
2601  Error("Undefined reference variable in ApplyLinkRotDeformX");
2602  }
2603 
2604  for(j=0;j<3;j++)
2605  {
2606  AddVariable2Monomial(NFUN,linkVars[j],1,&f);
2607  if (sv!=NO_UINT)
2608  AddVariable2Monomial(NFUN,sv,1,&f);
2609  AddVariable2Monomial(NFUN,linkVars[3],1,&f);
2610  AddCt2Monomial(sf*p[0],&f);
2611 
2612  AddMonomial(&f,&(eq[j]));
2613  ResetMonomial(&f);
2614  }
2615  }
2616  free(vname);
2617  DeleteMonomial(&f);
2618 }
2619 
2620 void ApplyLinkRotFLinks(double sf,unsigned int sv,double *p,Tequation *eq,
2621  TCuikSystem *cs,boolean groundLink,Tlink *l)
2622 {
2623  /*
2624  sf*sv*R*T(px py px) where T is defined from the linkVars (check that hasVars==TRUE)
2625  */
2626 
2627  Tmonomial f;
2628  unsigned int i;
2629 
2630  InitMonomial(&f);
2631  if (groundLink)
2632  {
2633  for(i=0;i<3;i++)
2634  {
2635  if (sv!=NO_UINT)
2636  AddVariable2Monomial(NFUN,sv,1,&f);
2637  AddCt2Monomial(sf*p[i],&f);
2638 
2639  AddMonomial(&f,&(eq[i]));
2640  ResetMonomial(&f);
2641  }
2642  }
2643  else
2644  {
2645  unsigned int j;
2646  unsigned int linkVars[9];
2647  char *vname;
2648  double pInt[3];
2649 
2650  HTransformApply(p,pInt,&(l->R));
2651 
2652  NEW(vname,strlen(l->name)+100,char);
2653 
2654  for(i=0;i<3;i++) /* vector */
2655  {
2656  for(j=0;j<3;j++) /* component */
2657  {
2658  LINK_ROT(vname,l->name,i,j);
2659  linkVars[i*3+j]=GetCSVariableID(vname,cs);
2660  if (linkVars[i*3+j]==NO_UINT)
2661  Error("Undefined reference variable in ApplyLinkRotFLinks");
2662  }
2663  }
2664 
2665  for(i=0;i<3;i++) /* component (row) */
2666  {
2667  for(j=0;j<3;j++) /* vector (column) */
2668  {
2669  if (fabs(pInt[j])>ZERO)
2670  {
2671  AddVariable2Monomial(NFUN,linkVars[j*3+i],1,&f);
2672  if (sv!=NO_UINT)
2673  AddVariable2Monomial(NFUN,sv,1,&f);
2674  AddCt2Monomial(sf*pInt[j],&f);
2675 
2676  AddMonomial(&f,&(eq[i]));
2677  ResetMonomial(&f);
2678  }
2679  }
2680  }
2681  free(vname);
2682  }
2683  DeleteMonomial(&f);
2684 }
2685 
2686 void ApplyLinkRotLinks(double sf,unsigned int sv,double *p,Tequation *eq,
2687  TCuikSystem *cs,boolean groundLink,Tlink *l)
2688 {
2689  /*
2690  sf*sv*R*T(px py px) where T is defined from the linkVars (check that hasVars==TRUE)
2691  */
2692 
2693  Tmonomial f;
2694 
2695  InitMonomial(&f);
2696  if (groundLink)
2697  {
2698  unsigned int i;
2699 
2700  for(i=0;i<3;i++)
2701  {
2702  if (sv!=NO_UINT)
2703  AddVariable2Monomial(NFUN,sv,1,&f);
2704  AddCt2Monomial(sf*p[i],&f);
2705 
2706  AddMonomial(&f,&(eq[i]));
2707  ResetMonomial(&f);
2708  }
2709  }
2710  else
2711  {
2712  unsigned int i,j,n;
2713  unsigned int linkVars[12];
2714  char *vname;
2715  double pInt[3];
2716 
2717  HTransformApply(p,pInt,&(l->R));
2718 
2719  NEW(vname,strlen(l->name)+100,char);
2720 
2721  for(i=0;i<4;i++) /*vector*/
2722  {
2723  for(j=0;j<3;j++) /*component*/
2724  {
2725  LINK_ROT2(vname,l->name,i,j);
2726  n=i*3+j;
2727  linkVars[n]=GetCSVariableID(vname,cs);
2728  if (linkVars[n]==NO_UINT)
2729  Error("Undefined reference variable in ApplyLinkRotPM");
2730  }
2731  }
2732 
2733  for(i=0;i<3;i++) /*row = component */
2734  {
2735  for(j=0;j<4;j++) /*column = vector */
2736  {
2737  if (fabs(pInt[(j<2?j:2)])>ZERO)
2738  {
2739  AddVariable2Monomial(NFUN,linkVars[j*3+i],1,&f);
2740  if (sv!=NO_UINT)
2741  AddVariable2Monomial(NFUN,sv,1,&f);
2742  AddCt2Monomial((j==3?-1:1)*sf*pInt[(j<2?j:2)],&f);
2743 
2744  AddMonomial(&f,&(eq[i]));
2745  ResetMonomial(&f);
2746  }
2747  }
2748  }
2749 
2750  free(vname);
2751  }
2752  DeleteMonomial(&f);
2753 }
2754 
2755 void ApplyLinkRotQLinks(double sf,unsigned int sv,double *p,Tequation *eq,
2756  TCuikSystem *cs,boolean groundLink,Tlink *l)
2757 {
2758  /*
2759  sf*sv*R*T(px py px) where T is defined from the linkVars (check that hasVars==TRUE)
2760  (R=Id for quaternions -> not used)
2761  */
2762  Tmonomial f;
2763  unsigned int i;
2764 
2765  /*Note that quaternions asume an orthonormal rotation matrix (i.e., we do not allow
2766  the change of the link reference)*/
2767 
2768  InitMonomial(&f);
2769  if (groundLink)
2770  {
2771  unsigned int j;
2772  /* Replace id by anoher matrix if necessary */
2773  double id[3][3]={{1.0,0.0,0.0},{0.0,1.0,0.0},{0.0,0.0,1.0}};
2774  double ct;
2775 
2776  for(i=0;i<3;i++) /* row */
2777  {
2778  ct=0.0;
2779  for(j=0;j<3;j++) /* column */
2780  ct+=id[i][j]*p[j];
2781 
2782  if (sv!=NO_UINT)
2783  AddVariable2Monomial(NFUN,sv,1,&f);
2784  AddCt2Monomial(sf*ct,&f);
2785 
2786  AddMonomial(&f,&(eq[i]));
2787  ResetMonomial(&f);
2788  }
2789  }
2790  else
2791  {
2792  unsigned int i,j,n;
2793  unsigned int linkVars[10];
2794  char *vname;
2795 
2796  /* indices of the two 'e' variables that define each
2797  element in the rotation matrix */
2798  unsigned int map[2][3][3]={
2799  {{7,5,6},
2800  {5,4,8},
2801  {6,8,4}},
2802  {{9,3,2},
2803  {3,9,1},
2804  {2,1,7}}};
2805  /* sign for each of the two 'e' variables that define
2806  each element of the rotation matrix. */
2807  double sg[2][3][3]={
2808  {{-1.0,+1.0,+1.0}, /* fist variable is alwoys + but for the diagonal */
2809  {+1.0,-1.0,+1.0},
2810  {+1.0,+1.0,-1.0}},
2811  {{-1.0,-1.0,+1.0},
2812  {+1.0,-1.0,-1.0},
2813  {-1.0,+1.0,-1.0}}};
2814 
2815  /*
2816  The rotation matrix is
2817  1-2*c^2-2*d^2 2*b*c-2*a*d 2*b*d+2*a*c
2818  2*b*c+2*a*d 1-2*b^2-2*d^2 2*c*d-2*a*b
2819  2*b*d-2*a*c 2*c*d+2*a*b 1-2*b^2-2*c^2
2820 
2821  1-2*e_2_2-2*e_3_3 2*e_1_2-2*e_0_3 2*e_1_3+2*e_0_2
2822  2*e_1_2+2*e_0_3 1-2*e_1_1-2*e_3_3 2*e_2_3-2*e_0_1
2823  2*e_1_3-2*e_0_2 2*e_2_3+2*e_0_1 1-2*e_1_1-2*e_2_2
2824 
2825  1-2*v[7]-2*v[9] 2*v[5]-2*v[3] 2*v[6]+2*v[2]
2826  2*v[5]+2*v[3] 1-2*v[4]-2*v[9] 2*v[8]-2*v[1]
2827  2*v[6]-2*v[2] 2*v[8]+2*v[1] 1-2*v[4]-2*v[7]
2828 
2829  */
2830  NEW(vname,strlen(l->name)+100,char);
2831 
2832  n=0;
2833  for(i=0;i<4;i++)
2834  {
2835  for(j=i;j<4;j++)
2836  {
2837  LINK_ROT3_E(vname,l->name,i,j);
2838  linkVars[n]=GetCSVariableID(vname,cs);
2839  if (linkVars[n]==NO_UINT)
2840  Error("Undefined reference variable in ApplyLinkRot");
2841  n++;
2842  }
2843  }
2844 
2845  /* The ones in the diagonal */
2846  for(i=0;i<3;i++)
2847  {
2848  if (sv!=NO_UINT)
2849  AddVariable2Monomial(NFUN,sv,1,&f);
2850  AddCt2Monomial(sf*p[i],&f);
2851 
2852  AddMonomial(&f,&(eq[i]));
2853  ResetMonomial(&f);
2854  }
2855 
2856  for(n=0;n<2;n++) /* each rotation matix is the result of operation to 'e' variables */
2857  {
2858  for(i=0;i<3;i++) /*row*/
2859  {
2860  for(j=0;j<3;j++) /*column*/
2861  {
2862  if (fabs(p[j])>ZERO)
2863  {
2864  AddVariable2Monomial(NFUN,linkVars[map[n][i][j]],1,&f);
2865  if (sv!=NO_UINT)
2866  AddVariable2Monomial(NFUN,sv,1,&f);
2867  AddCt2Monomial(2.0*sg[n][i][j]*sf*p[j],&f);
2868 
2869  AddMonomial(&f,&(eq[i]));
2870  ResetMonomial(&f);
2871  }
2872  }
2873  }
2874  }
2875 
2876  free(vname);
2877  }
2878  DeleteMonomial(&f);
2879 }
2880 
2881 void ApplyLinkRotVar(Tparameters *pr,double sf,unsigned int *vID,Tequation *eq,
2882  TCuikSystem *cs,boolean groundLink,Tlink *l)
2883 {
2884  unsigned int r;
2885 
2886  r=(unsigned int)(GetParameter(CT_REPRESENTATION,pr));
2887  if ((r!=REP_JOINTS)&&(fabs(sf)>ZERO))
2888  {
2889  Tequation u[3],v[3],w[3];
2890  unsigned int i,j;
2891  double ct[3]={1,0,0};
2892 
2893  switch(l->type)
2894  {
2895  case LINK_AxisX:
2896  if (((vID[1]!=NO_UINT)||(vID[2]!=NO_UINT)))
2897  Error("ApplyLinkRotVar only works on points on X for Axis_X links");
2898 
2899  for(i=0;i<3;i++)
2900  InitEquation(&(u[i]));
2901 
2902  for(i=0;i<3;i++)
2903  ApplyLinkRot(pr,sf,NO_UINT,ct,u,cs,groundLink,l);
2904 
2905  for(i=0;i<3;i++)
2906  VarAccumulateEquations(&(u[i]),vID[0],&(eq[i]));
2907 
2908  for(i=0;i<3;i++)
2909  DeleteEquation(&(u[i]));
2910  break;
2911  case LINK_DeformX:
2912  case LINK_PrismaticX:
2913  {
2914  unsigned int lvID[MAX_ROT_VARS];
2915 
2916  if (((vID[1]!=NO_UINT)||(vID[2]!=NO_UINT)))
2917  Error("ApplyLinkRotVar only works on points on X for Deform_X links");
2918 
2919  for(i=0;i<3;i++)
2920  InitEquation(&(u[i]));
2921 
2922  /* ensure that no one accessing the link information
2923  in parallel invalidates the cache while we use it*/
2924 
2925  for(i=0;i<3;i++)
2926  ApplyLinkRot(pr,sf,NO_UINT,ct,u,cs,groundLink,l);
2927 
2928  GetRotVarID(r,cs,lvID,l);
2929  for(i=0;i<3;i++)
2930  {
2931  VarScaleEquation(lvID[3],&(u[i])); /* deformation parameter */
2932  VarAccumulateEquations(&(u[i]),vID[0],&(eq[i]));
2933  }
2934  for(i=0;i<3;i++)
2935  DeleteEquation(&(u[i]));
2936  }
2937  break;
2938  case LINK_FullRot:
2939  for(i=0;i<3;i++)
2940  {
2941  InitEquation(&(u[i]));
2942  InitEquation(&(v[i]));
2943  InitEquation(&(w[i]));
2944  }
2945  /* We have to compute M*R*[vID]
2946  M=matrix given by the link variables (=M*iR)
2947  R=Basis change
2948  vID=given variable vector
2949  This is computed as (M*R)*[vID]
2950  First [u v w]=M*R
2951  u=M*R_1 (first column or R)
2952  v=M*R_2 (second column or R)
2953  w=M*R_3 (third column or R)
2954  and then [u v w]*[vID] -> linear combination of u,v,w
2955  */
2956  /* For LINK_AxisX links R is the identity. */
2957  for(i=0;i<3;i++)
2958  {
2959  for(j=0;j<3;j++)
2960  ct[j]=l->R[j][i];
2961  ApplyLinkRot(pr,sf,NO_UINT,ct,(i==0?u:(i==1?v:w)),cs,groundLink,l);
2962  }
2963 
2964  for(i=0;i<3;i++)
2965  {
2966  VarAccumulateEquations(&(u[i]),vID[0],&(eq[i]));
2967  VarAccumulateEquations(&(v[i]),vID[1],&(eq[i]));
2968  VarAccumulateEquations(&(w[i]),vID[2],&(eq[i]));
2969  }
2970  for(i=0;i<3;i++)
2971  {
2972  DeleteEquation(&(u[i]));
2973  DeleteEquation(&(v[i]));
2974  DeleteEquation(&(w[i]));
2975  }
2976  break;
2977  case LINK_NoRot:
2978  break;
2979  default:
2980  Error("Unkown link type in ApplyLinkRotVar");
2981  }
2982  }
2983 }
2984 
2986  boolean groundLink,Tlink *l)
2987 {
2988  unsigned int r;
2989 
2990  /* in joints all variables are system */
2991  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2992  if (r!=REP_JOINTS)
2993  {
2994  if (l->type==LINK_FullRot) /* NoRot, AxisX and DeformX links do not need regeneration
2995  (don't generate dummy variables) */
2996  {
2997  switch(r)
2998  {
2999  case REP_LINKS:
3000  RegenerateLinkSolutionLinks(r,cs,sol,groundLink,l);
3001  break;
3002  case REP_FLINKS:
3003  /* When using 9 variables all variables used to represent the
3004  rotation of the link are system variables and, thus, no
3005  dummy has to be computed.*/
3006  break;
3007  case REP_QLINKS:
3008  RegenerateLinkSolutionQLinks(r,cs,sol,groundLink,l);
3009  break;
3010  default:
3011  Error("Undefined representation type in RegenerateLinkSolution");
3012  }
3013  }
3014  RegenerateLinkSolutionConf(r,cs,sol,l);
3015  }
3016 }
3017 
3018 void RegenerateLinkSolutionConf(unsigned int r,TCuikSystem *cs,double *sol,Tlink *l)
3019 {
3020  /* nothing need to be done (all internal conf. variables, if any, are already system variables) */
3021 }
3022 
3023 void RegenerateLinkSolutionLinks(unsigned int r,TCuikSystem *cs,double *sol,boolean groundLink,Tlink *l)
3024 {
3025  if (!groundLink)
3026  {
3027  unsigned int vID[MAX_ROT_VARS];
3028 
3029  GetRotVarID(r,cs,vID,l);
3030 
3031  /* compute w and wp from u and v (recall that u X v = s*(w - wp) )*/
3032  sol[vID[6]]=sol[vID[1]]*sol[vID[5]]/l->s;
3033  sol[vID[7]]=sol[vID[2]]*sol[vID[3]]/l->s;
3034  sol[vID[8]]=sol[vID[0]]*sol[vID[4]]/l->s;
3035 
3036  sol[vID[9]] =sol[vID[2]]*sol[vID[4]]/l->s;
3037  sol[vID[10]]=sol[vID[0]]*sol[vID[5]]/l->s;
3038  sol[vID[11]]=sol[vID[1]]*sol[vID[3]]/l->s;
3039  }
3040 }
3041 
3042 void RegenerateLinkSolutionQLinks(unsigned int r,TCuikSystem *cs,double *sol,boolean groundLink,Tlink *l)
3043 {
3044  if (!groundLink)
3045  {
3046  unsigned int i,j,n;
3047  double qVars[4];
3048  unsigned int vID[MAX_ROT_VARS];
3049 
3050  GetRotVarID(r,cs,vID,l);
3051 
3052  for(j=0;j<4;j++)
3053  qVars[j]=sol[vID[j]];
3054 
3055  n=4;
3056  for(i=0;i<4;i++)
3057  {
3058  for(j=i;j<4;j++)
3059  {
3060  sol[vID[n]]=qVars[i]*qVars[j];
3061  n++;
3062  }
3063  }
3064  }
3065 }
3066 
3068  boolean groundLink,Tlink *l)
3069 {
3070  unsigned int r;
3071 
3072  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
3073  if (r!=REP_JOINTS)
3074  {
3075  if (l->type==LINK_FullRot) /* NoRot, AxisX and DeformX links do not need regeneration
3076  (don't generate dummy variables) */
3077  {
3078  unsigned int r;
3079 
3080  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
3081  switch(r)
3082  {
3083  case REP_LINKS:
3084  RegenerateLinkBoxLinks(r,cs,b,groundLink,l);
3085  break;
3086  case REP_FLINKS:
3087  /* When using 9 variables all variables used to represent the
3088  rotation of the link are system variables and, thus, no
3089  dummy has to be computed.*/
3090  break;
3091  case REP_QLINKS:
3092  RegenerateLinkBoxQLinks(r,cs,b,groundLink,l);
3093  break;
3094  default:
3095  Error("Undefined representation type in RegenerateLinkBox");
3096  }
3097  }
3098  RegenerateLinkBoxConf(r,cs,b,l);
3099  }
3100 }
3101 
3102 void RegenerateLinkBoxConf(unsigned int r,TCuikSystem *cs,Tbox *b,Tlink *l)
3103 {
3104  /* nothing need to be done (all internal conf. variables, if any, are already system variables) */
3105 }
3106 
3107 void RegenerateLinkBoxLinks(unsigned int r,TCuikSystem *cs,Tbox *b,boolean groundLink,Tlink *l)
3108 {
3109  if (!groundLink)
3110  {
3111  Tinterval range,*is;
3112  unsigned int vID[MAX_ROT_VARS];
3113 
3114  GetRotVarID(r,cs,vID,l);
3115 
3116  is=GetBoxIntervals(b);
3117 
3118  /* compute w and wp from u and v (recall that u X v = s*(w - wp)) */
3119  IntervalProduct(&(is[vID[1]]),&(is[vID[5]]),&range);
3120  IntervalScale(&range,1/l->s,&range);
3121  SetBoxInterval(vID[6],&range,b);
3122 
3123  IntervalProduct(&(is[vID[2]]),&(is[vID[3]]),&range);
3124  IntervalScale(&range,1/l->s,&range);
3125  SetBoxInterval(vID[7],&range,b);
3126 
3127  IntervalProduct(&(is[vID[0]]),&(is[vID[4]]),&range);
3128  IntervalScale(&range,1/l->s,&range);
3129  SetBoxInterval(vID[8],&range,b);
3130 
3131  IntervalProduct(&(is[vID[2]]),&(is[vID[4]]),&range);
3132  IntervalScale(&range,1/l->s,&range);
3133  SetBoxInterval(vID[9],&range,b);
3134 
3135  IntervalProduct(&(is[vID[0]]),&(is[vID[5]]),&range);
3136  IntervalScale(&range,1/l->s,&range);
3137  SetBoxInterval(vID[10],&range,b);
3138 
3139  IntervalProduct(&(is[vID[1]]),&(is[vID[3]]),&range);
3140  IntervalScale(&range,1/l->s,&range);
3141  SetBoxInterval(vID[11],&range,b);
3142  }
3143 }
3144 
3145 void RegenerateLinkBoxQLinks(unsigned int r,TCuikSystem *cs,Tbox *b,boolean groundLink,Tlink *l)
3146 {
3147  if (!groundLink)
3148  {
3149  unsigned int i,j,n;
3150  Tinterval qVars[4];
3151  Tinterval range;
3152  unsigned int vID[MAX_ROT_VARS];
3153 
3154  GetRotVarID(r,cs,vID,l);
3155 
3156  for(j=0;j<4;j++)
3157  CopyInterval(&(qVars[j]),GetBoxInterval(vID[j],b));
3158 
3159  n=4;
3160  for(i=0;i<4;i++)
3161  {
3162  for(j=i;j<4;j++)
3163  {
3164  IntervalProduct(&(qVars[i]),&(qVars[j]),&range);
3165  SetBoxInterval(vID[n],&range,b);
3166  n++;
3167  }
3168  }
3169  }
3170 }
3171 
3173  boolean groundLink,double *trans,THTransform *t,
3174  TLinkConf *def,Tlink *l)
3175 {
3176  unsigned int r;
3177 
3178  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
3179  if (r!=REP_JOINTS)
3180  {
3181  switch(l->type)
3182  {
3183  case LINK_AxisX:
3184  case LINK_DeformX:
3185  case LINK_PrismaticX:
3186  GetTransform2LinkAxisX(r,cs,sol,groundLink,trans,t,l);
3187  break;
3188  case LINK_FullRot:
3189  switch(r)
3190  {
3191  case REP_LINKS:
3192  GetTransform2LinkLinks(r,cs,sol,groundLink,trans,t,l);
3193  break;
3194  case REP_FLINKS:
3195  GetTransform2LinkFLinks(r,cs,sol,groundLink,trans,t,l);
3196  break;
3197  case REP_QLINKS:
3198  GetTransform2LinkQLinks(r,cs,sol,groundLink,trans,t,l);
3199  break;
3200  default:
3201  Error("Undefined representation type in GetTransform2Link");
3202  }
3203  break;
3204  case LINK_NoRot:
3205  HTransformTxyz(trans[0],trans[1],trans[2],t);
3206  break;
3207  default:
3208  Error("Unkown link type in GetTransform2Link");
3209  }
3210 
3211  GetLinkConfFromSolution(r,cs,sol,def,l);
3212  }
3213 }
3214 
3215 void GetTransform2LinkAxisX(unsigned int r,TCuikSystem *cs,double *sol,boolean groundLink,
3216  double *trans,THTransform *t,Tlink *l)
3217 {
3218  if (groundLink)
3219  HTransformIdentity(t);
3220  else
3221  {
3222  unsigned int i;
3223  double u[3],v[3],w[3];
3224  unsigned int vID[MAX_ROT_VARS];
3225 
3226  GetRotVarID(r,cs,vID,l);
3227 
3228  /* We only have the X vector and, thus, we define an arbitrary, but
3229  correct reference system */
3230  for(i=0;i<3;i++)
3231  {
3232  u[i]=sol[vID[i]];
3233  if (fabs(u[i])<ZERO) u[i]=0.0;
3234  }
3235  Normalize(3,u);
3236 
3237  DefineNormalVector(v,u);
3238  CrossProduct(u,v,w);
3239 
3240  HTransformIdentity(t);
3241  for(i=0;i<3;i++)
3242  {
3243  HTransformSetElement(i,AXIS_X,u[i],t);
3244  HTransformSetElement(i,AXIS_Y,v[i],t);
3245  HTransformSetElement(i,AXIS_Z,w[i],t);
3246  HTransformSetElement(i,AXIS_H,trans[i],t);
3247  }
3248  }
3249 }
3250 
3251 void GetTransform2LinkFLinks(unsigned int r,TCuikSystem *cs,double *sol,boolean groundLink,
3252  double *trans,THTransform *t,Tlink *l)
3253 {
3254  if (groundLink)
3255  HTransformIdentity(t);
3256  else
3257  {
3258  unsigned int i,j;
3259  unsigned int vID[MAX_ROT_VARS];
3260 
3261  GetRotVarID(r,cs,vID,l);
3262 
3263  HTransformIdentity(t);
3264 
3265  /* Define the homogeneous transform using the rotation and the translation.*/
3266  for(i=0;i<3;i++)
3267  {
3268  HTransformSetElement(i,AXIS_H,trans[i],t);
3269  for(j=0;j<3;j++)
3270  HTransformSetElement(i,j,sol[vID[i+j*3]],t);
3271  }
3272 
3273  /* The link variables representing the link rotation encode
3274  M*iR
3275  M = orthonormal rotation matrix
3276  iR= basis change to obtain a more numerically safe basis
3277  The orthonormal matrix is obtained by multiplying
3278  (M*iR)*R
3279 
3280  See ChangeLinkReferenceFrame
3281  */
3282  HTransformProduct(t,&(l->R),t);
3283  /* This ensures that the transformation is actually rigid
3284  (fixes numerical errors and the error of taking the center
3285  of the solution box as a proper solution) */
3287  }
3288 }
3289 
3290 void GetTransform2LinkLinks(unsigned int r,TCuikSystem *cs,double *sol,boolean groundLink,
3291  double *trans,THTransform *t,Tlink *l)
3292 {
3293  if (groundLink)
3294  HTransformIdentity(t);
3295  else
3296  {
3297  unsigned int i,j;
3298  unsigned int vID[MAX_ROT_VARS];
3299 
3300  GetRotVarID(r,cs,vID,l);
3301 
3302  /* Define the homogeneous transform using the rotation and the translation.*/
3303  HTransformIdentity(t);
3304  for(i=0;i<3;i++) /* component */
3305  {
3306  HTransformSetElement(i,AXIS_H,trans[i],t);
3307  for(j=0;j<3;j++) /* vector */
3308  {
3309  if (j<2)
3310  HTransformSetElement(i,j,sol[vID[i+j*3]],t);
3311  else
3312  HTransformSetElement(i,j,(sol[vID[i+j*3]]-sol[vID[i+(j+1)*3]]),t);
3313  }
3314  }
3315 
3316  /* The link variables representing the link rotation encode
3317  M*iR
3318  M = orthonormal rotation matrix
3319  iR= basis change to obtain a more numerically safe basis
3320  The orthonormal matrix is obtained by multiplying
3321  (M*iR)*R
3322 
3323  See ChangeLinkReferenceFrame
3324  */
3325  HTransformProduct(t,&(l->R),t);
3326  /* This ensures that the transformation is actually rigid
3327  (fixes numerical errors and the error of taking the center
3328  of the solution box as a proper solution) */
3330  }
3331 }
3332 
3333 
3334 void GetTransform2LinkQLinks(unsigned int r,TCuikSystem *cs,double *sol,boolean groundLink,
3335  double *trans,THTransform *t,Tlink *l)
3336 {
3337  if (groundLink)
3338  HTransformIdentity(t);
3339  else
3340  {
3341  double e[4][4]; /* only the upper half is used */
3342  unsigned int i,j,n;
3343  unsigned int vID[MAX_ROT_VARS];
3344 
3345  GetRotVarID(r,cs,vID,l);
3346 
3347  n=4;
3348  for(i=0;i<4;i++)
3349  {
3350  for(j=i;j<4;j++)
3351  {
3352  e[i][j]=2*sol[vID[n]];
3353  n++;
3354  }
3355  }
3356 
3357  /* Define the homogeneous transform using the rotation and the translation.*/
3358  HTransformIdentity(t);
3359 
3360  HTransformSetElement(0,0,1-e[2][2]-e[3][3],t);
3361  HTransformSetElement(0,1, e[1][2]+e[0][3],t);
3362  HTransformSetElement(0,2, e[1][3]-e[0][2],t);
3363  HTransformSetElement(0,3,trans[0],t);
3364 
3365  HTransformSetElement(1,0, e[1][2]-e[0][3],t);
3366  HTransformSetElement(1,1,1-e[1][1]-e[3][3],t);
3367  HTransformSetElement(1,2, e[2][3]+e[0][1],t);
3368  HTransformSetElement(1,3,trans[1],t);
3369 
3370  HTransformSetElement(2,0, e[1][3]+e[0][2],t);
3371  HTransformSetElement(2,1, e[2][3]-e[0][1],t);
3372  HTransformSetElement(2,2,1-e[1][1]-e[2][2],t);
3373  HTransformSetElement(2,3,trans[2],t);
3374 
3375  /* No basis change (R) is used in Quaternion representation */
3376 
3377  /* This ensures that the transformation is actually rigid */
3379  }
3380 }
3381 
3383  TCuikSystem *cs,double *sol,boolean groundLink,Tlink *l)
3384 {
3385  unsigned int r;
3386 
3387  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
3388  if (r!=REP_JOINTS)
3389  {
3390  switch(l->type)
3391  {
3392  case LINK_AxisX:
3393  case LINK_DeformX:
3394  case LINK_PrismaticX:
3395  GenerateLinkSolutionAxisX(r,t,cs,sol,groundLink,l);
3396  break;
3397  case LINK_FullRot:
3398  switch(r)
3399  {
3400  case REP_LINKS:
3401  GenerateLinkSolutionLinks(r,t,cs,sol,groundLink,l);
3402  break;
3403  case REP_FLINKS:
3404  GenerateLinkSolutionFLinks(r,t,cs,sol,groundLink,l);
3405  break;
3406  case REP_QLINKS:
3407  GenerateLinkSolutionQLinks(r,t,cs,sol,groundLink,l);
3408  break;
3409  default:
3410  Error("Undefined representation type in GenerateLinkSolution");
3411  }
3412  break;
3413  case LINK_NoRot:
3414  break;
3415  default:
3416  Error("Unknown link type in GenerateLinkSolution");
3417  }
3418 
3419  GenerateSolutionFromLinkConf(r,cs,def,sol,l);
3420  }
3421 }
3422 
3424  double *sol,boolean groundLink,Tlink *l)
3425 {
3426  if (!groundLink)
3427  {
3428  unsigned int j;
3429  unsigned int vID[MAX_ROT_VARS];
3430 
3431  GetRotVarID(r,cs,vID,l);
3432 
3433  for(j=0;j<3;j++)
3434  sol[vID[j]]=HTransformGetElement(j,0,t);
3435  }
3436 }
3437 
3439  double *sol,boolean groundLink,Tlink *l)
3440 {
3441  if (!groundLink)
3442  {
3443  THTransform it;
3444  unsigned int i,j,n;
3445  unsigned int vID[MAX_ROT_VARS];
3446 
3447  /* The link variables encode (M*iR) and the given 't' is only M */
3448  HTransformProduct(t,&(l->iR),&it);
3449 
3450  GetRotVarID(r,cs,vID,l);
3451 
3452  n=0;
3453  for(i=0;i<3;i++) /*vector*/
3454  {
3455  for(j=0;j<3;j++) /*component*/
3456  {
3457  sol[vID[n]]=HTransformGetElement(j,i,&it);
3458  n++;
3459  }
3460  }
3461  HTransformDelete(&it);
3462  }
3463 }
3464 
3466  double *sol,boolean groundLink,Tlink *l)
3467 {
3468  if (!groundLink)
3469  {
3470  THTransform it;
3471  unsigned int i,j,n;
3472  unsigned int vID[MAX_ROT_VARS];
3473 
3474  /* The link variables encode (M*iR) and the given 't' is only M */
3475  HTransformProduct(t,&(l->iR),&it);
3476 
3477  GetRotVarID(r,cs,vID,l);
3478 
3479  n=0;
3480  for(i=0;i<2;i++) /*vector*/
3481  {
3482  for(j=0;j<3;j++) /*componet*/
3483  {
3484  sol[vID[n]]=HTransformGetElement(j,i,&it);
3485  n++;
3486  }
3487  }
3488  HTransformDelete(&it);
3489 
3490  RegenerateLinkSolutionLinks(r,cs,sol,groundLink,l);
3491  }
3492 }
3493 
3495  double *sol,boolean groundLink,Tlink *l)
3496 {
3497  if (!groundLink)
3498  {
3499  double u,s,q;
3500  unsigned int vID[MAX_ROT_VARS];
3501 
3502  GetRotVarID(r,cs,vID,l);
3503 
3504  /* Recover the 4 quaternion elements from the transformation
3505  matrix*/
3506  q=HTransformGetElement(0,0,t)+
3507  HTransformGetElement(1,1,t)+
3508  HTransformGetElement(2,2,t);
3509  u=sqrt(1.0+q);
3510  s=0.5/u;
3511 
3512  sol[vID[0]]=0.5*u;
3513  sol[vID[1]]=(HTransformGetElement(2,1,t)-HTransformGetElement(1,2,t))*s;
3514  sol[vID[2]]=(HTransformGetElement(0,2,t)-HTransformGetElement(2,0,t))*s;
3515  sol[vID[3]]=(HTransformGetElement(1,0,t)-HTransformGetElement(0,1,t))*s;
3516 
3517  RegenerateLinkSolutionQLinks(r,cs,sol,groundLink,l);
3518  }
3519 }
3520 
3521 boolean VisibleLink(Tlink *l)
3522 {
3523  /* A visible link is a link with at least one visible body */
3524  boolean found;
3525 
3526  found=FALSE;
3527 
3528  if (l->type!=LINK_NoRot)
3529  {
3530  unsigned int i,n;
3531 
3532  n=LinkNBodies(l);
3533 
3534  for(i=0;((i<n)&&(!found));i++)
3536  }
3537  return(found);
3538 }
3539 
3541 {
3542  return(l->maxCoord);
3543 }
3544 
3545 void LinkPrintAtoms(FILE *f,THTransform *tl,Tlink *l)
3546 {
3547  unsigned int i,m;
3548  Tpolyhedron *bd;
3549 
3550  m=LinkNBodies(l);
3551 
3552  if ((m>0)&&(VisibleLink(l)))
3553  {
3554  for(i=0;i<m;i++)
3555  {
3556  bd=GetLinkBody(i,l);
3557  PolyhedronPrintCenter(f,tl,bd);
3558  }
3559  }
3560 }
3561 
3562 void LinkStoreAtoms(FILE *f,THTransform *tl,Tlink *l)
3563 {
3564  unsigned int i,m;
3565  Tpolyhedron *bd;
3566 
3567  m=LinkNBodies(l);
3568 
3569  if ((m>0)&&(VisibleLink(l)))
3570  {
3571  for(i=0;i<m;i++)
3572  {
3573  bd=GetLinkBody(i,l);
3575  }
3576  }
3577 }
3578 
3579 void PlotLink(Tplot3d *pt,double axesLength,Tlink *l)
3580 {
3581  unsigned int n,i;
3582 
3583  n=LinkNBodies(l);
3584 
3585  if ((n>0)&&(VisibleLink(l)))
3586  {
3587  for(i=0;i<n;i++)
3588  PlotPolyhedron(pt,GetLinkBody(i,l));
3589  }
3590 
3591  /* default no axis shown */
3592  for(i=0;i<3;i++)
3593  l->axisID[i]=NO_UINT;
3594 
3595  if (axesLength>0)
3596  {
3597  double points[3][2]={{0,0},{0,0},{0,0}};
3598  Tcolor axesColor;
3599 
3600  for(i=0;i<((l->type!=LINK_AxisX)||(l->type!=LINK_DeformX)||(l->type==LINK_PrismaticX)?3:1);i++)
3601  {
3602  NewColor((i==0?1:0),(i==1?1:0),(i==2?1:0),&axesColor);
3603  l->axisID[i]=StartNew3dObject(&axesColor,pt);
3604  DeleteColor(&axesColor);
3605 
3606  points[i][1]=axesLength;
3607  PlotVect3d(2,points[0],points[1],points[2],pt);
3608  points[i][1]=0;
3609 
3610  Close3dObject(pt);
3611  }
3612  }
3613 }
3614 
3616 {
3617  unsigned int m,i;
3618 
3619  m=LinkNBodies(l);
3620 
3621  for(i=0;i<3;i++)
3622  {
3623  if (l->axisID[i]!=NO_UINT)
3624  Move3dObject(l->axisID[i],t,pt);
3625  }
3626 
3627  if ((m>0)&&(VisibleLink(l)))
3628  {
3629  Tpolyhedron *bd;
3630  THTransform td;
3631  boolean intConf;
3632 
3633  intConf=(!EmptyLinkConf(def));
3634 
3635  for(i=0;i<m;i++)
3636  {
3637  bd=GetLinkBody(i,l);
3638  if (intConf)
3639  {
3640  /* internal body configuration (displacement/deformation) */
3641  GetLinkConfTransform(i,&td,def);
3642  HTransformProduct(t,&td,&td);
3643  MovePolyhedron(pt,&td,bd);
3644  HTransformDelete(&td);
3645  }
3646  else
3647  MovePolyhedron(pt,t,bd);
3648  }
3649  }
3650 }
3651 
3652 void PrintLink(FILE *f,char *path,char *prefix,Tlink *l)
3653 {
3654  /* only FR links are printed. The rest of links are
3655  not defined in the world files but automatically
3656  generated from joints. They will be printed when
3657  printing the joints. */
3658  if (l->type==LINK_FullRot)
3659  {
3660  unsigned int i;
3661  unsigned int n;
3662  Tpolyhedron *b;
3663  char *label;
3664 
3665  if (prefix==NULL)
3666  label=l->name;
3667  else
3668  {
3669  unsigned int ln,lp;
3670 
3671  ln=strlen(l->name);
3672  lp=strlen(prefix);
3673  NEW(label,ln+2+lp,char);
3674  sprintf(label,"%s_%s",prefix,l->name);
3675  label[lp+ln+1]=0;
3676  }
3677 
3678  fprintf(f," %s ",l->name);
3679  n=LinkNBodies(l);
3680  if (n>0)
3681  fprintf(f,": ");
3682  for(i=0;i<n;i++)
3683  {
3684  b=GetLinkBody(i,l);
3685  PrintPolyhedron(f,path,label,i,b);
3686  }
3687  fprintf(f,"\n");
3688 
3689  if (prefix!=NULL)
3690  free(label);
3691  }
3692 }
3693 
3695 {
3696  unsigned int i,n;
3697  Tpolyhedron *b;
3698 
3699  if (l->name!=NULL)
3700  free(l->name);
3701 
3702  /*Bodies are not deleted since mechanisms take care of them
3703  (here we only have IDs of the bodies)*/
3704 
3705  n=LinkNBodies(l);
3706  for(i=0;i<n;i++)
3707  {
3708  b=GetLinkBody(i,l);
3709  DeletePolyhedron(b);
3710  free(b);
3711  }
3712  DeleteVector(&(l->bodies));
3713 
3714  HTransformDelete(&(l->R));
3715  HTransformDelete(&(l->iR));
3716 
3717  if (l->csvID!=NULL)
3718  {
3719  free(l->vID);
3720  l->vID=NULL;
3721  l->csvID=NULL;
3722  l->nvID=0;
3723  }
3724  l->varLength=FALSE;
3725  DeleteInterval(&(l->length));
3726  l->forceModel=NO_FORCE;
3727  l->stiffness=0;
3728  DeleteInterval(&(l->rest));
3729  DeleteInterval(&(l->force));
3730 }
#define AXIS_X
One of the dimension of R^3.
Definition: htransform.h:83
void PlotVect3d(unsigned int n, double *x, double *y, double *z, Tplot3d *p)
Adds a polyline to the current object.
Definition: plot3d.c:447
boolean Intersection(Tinterval *i1, Tinterval *i2, Tinterval *i_out)
Computes the intersection of two intervals.
Definition: interval.c:285
void DeleteVector(void *vector)
Destructor.
Definition: vector.c:389
Tinterval * GetBoxInterval(unsigned int n, Tbox *b)
Returns a pointer to one of the intervals defining the box.
Definition: box.c:270
Definition of basic functions.
#define SYSTEM_EQ
One of the possible type of equations.
Definition: equation.h:146
#define REP_JOINTS
One of the possible values of the REPRESENTATION parameter.
Definition: parameters.h:60
unsigned int VectorSize(Tvector *vector)
Gets the number of elements in a vector.
Definition: vector.c:173
#define FALSE
FALSE.
Definition: boolean.h:30
boolean EmptyPolyhedron(Tpolyhedron *p)
Used for objects that failed to initialize.
Definition: polyhedron.c:856
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
void GenerateSaddleEquation(unsigned int vx, unsigned int vy, unsigned int vz, Tequation *eq)
Construtor. Generates a saddle equation.
Definition: equation.c:1455
#define REP_FLINKS
One of the possible values of the REPRESENTATION parameter.
Definition: parameters.h:37
#define REP_LINKS
One of the possible values of the REPRESENTATION parameter.
Definition: parameters.h:27
void SetEquationType(unsigned int type, Tequation *eq)
Changes the type of the equation (SYSTEM_EQ, CARTESIAN_EQ, DUMMY_EQ, DERIVED_EQ). ...
Definition: equation.c:1076
void GenerateCrossProductEquations(unsigned int v1x, unsigned int v1y, unsigned int v1z, unsigned int v2x, unsigned int v2y, unsigned int v2z, unsigned int v3x, unsigned int v3y, unsigned int v3z, unsigned int vs, double s, Tequation *eq)
Construtor. Generates the three equations of the cross product of two unitary vectors.
Definition: equation.c:1527
#define NEW(_var, _n, _type)
Allocates memory space.
Definition: defines.h:385
#define LINK_ROT3_Q(vname, linkName, j)
System variables for a rotation matrix using quaternions.
Definition: varnames.h:153
void GenerateNormEquation(unsigned int vx, unsigned int vy, unsigned int vz, double n, Tequation *eq)
Construtor. Generates an equation that is the norm of a 3d vector.
Definition: equation.c:1494
void PolyhedronPrintCenterAndRadius(FILE *f, THTransform *t, Tpolyhedron *p)
Prints the center and the radius of a sphere to a file.
Definition: polyhedron.c:1505
#define LINK_LENGTH(vname, linkName)
Link length.
Definition: varnames.h:97
void DeleteEquation(Tequation *eq)
Destructor.
Definition: equation.c:1859
void * GetVectorElement(unsigned int i, Tvector *vector)
Returns a pointer to a vector element.
Definition: vector.c:270
A homgeneous transform in R^3.
void SetBoxInterval(unsigned int n, Tinterval *is, Tbox *b)
Replaces a particular interval in a box.
Definition: box.c:259
#define AXIS_Y
One of the dimension of R^3.
Definition: htransform.h:91
#define SYSTEM_VAR
One of the possible type of variables.
Definition: variable.h:24
unsigned int ndp
Definition: link.h:278
CBLAS_INLINE double Norm(unsigned int s, double *v)
Computes the norm of a vector.
unsigned int AddVariable2CS(Tvariable *v, TCuikSystem *cs)
Adds a variable to the system.
Definition: cuiksystem.c:2532
void IntervalAdd(Tinterval *i1, Tinterval *i2, Tinterval *i_out)
Addition of two intervals.
Definition: interval.c:423
void PlotPolyhedron(Tplot3d *pt, Tpolyhedron *p)
Adds the polyhedron to a 3D geometry.
Definition: polyhedron.c:1463
Definition of variable names.
#define EQU
In a Tequation, the equation relational operator is equal.
Definition: equation.h:202
#define LINK_ROT(vname, linkName, vn, cn)
Frame of reference for a link.
Definition: varnames.h:68
char * GetCSVariableName(unsigned int id, TCuikSystem *cs)
Gets a variable name.
Definition: cuiksystem.c:2612
#define LINK_REST(vname, linkName)
Rest variable.
Definition: varnames.h:108
void SetVariableInterval(Tinterval *i, Tvariable *v)
Sets the new range for the variable.
Definition: variable.c:68
#define TRUE
TRUE.
Definition: boolean.h:21
CBLAS_INLINE void Normalize(unsigned int s, double *v)
Normalizes a vector.
void InitEquation(Tequation *eq)
Constructor.
Definition: equation.c:86
void PrintPolyhedron(FILE *f, char *path, char *label, unsigned int n, Tpolyhedron *p)
Stores the polyhedron information into a file.
Definition: polyhedron.c:1574
void Error(const char *s)
General error function.
Definition: error.c:80
void InitTransSeq(TTransSeq *ts)
Constructor.
Definition: trans_seq.c:450
#define NFUN
No trigonometric function for the variable.
Definition: variable_set.h:36
A color.
Definition: color.h:86
#define LINK_ROT3_E(vname, linkName, rn, cn)
Frame of reference for a link using quaternions.
Definition: varnames.h:138
#define AXIS_H
The homogeneous dimension in R^3.
Definition: htransform.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
void CopyInterval(Tinterval *i_dst, Tinterval *i_org)
Copy constructor.
Definition: interval.c:59
void SetEquationValue(double v, Tequation *eq)
Changes the right-hand value of the equation.
Definition: equation.c:1089
void CopyVoidPtr(void *a, void *b)
Copy constructor for void pointers.
Definition: vector.c:87
void Move3dObject(unsigned int nobj, THTransform *t, Tplot3d *p)
Moves a 3d object.
Definition: plot3d.c:217
void AddMonomial(Tmonomial *f, Tequation *eq)
Adds a new monomial to the equation.
Definition: equation.c:1419
A polyhedron.
Definition: polyhedron.h:134
A 3D plot.
Definition: plot3d.h:54
double HTransformGetElement(unsigned int i, unsigned int j, THTransform *t)
Gets an element in a homogeneous transform.
Definition: htransform.c:329
void SetEquationCmp(unsigned int cmp, Tequation *eq)
Changes the relational operator (LEQ, GEQ, EQU) of the equation.
Definition: equation.c:1081
void ResetMonomial(Tmonomial *f)
Reset the monomial information.
Definition: monomial.c:24
double LowerLimit(Tinterval *i)
Gets the lower limit.
Definition: interval.c:79
#define AXIS_Z
One of the dimension of R^3.
Definition: htransform.h:99
#define TX
One of the types of homogeneous transforms in R^3.
Definition: htransform.h:35
void VarScaleEquation(unsigned int v, Tequation *eq)
Scales an equation with a variable factor.
Definition: equation.c:680
An equation.
Definition: equation.h:237
void DeleteVariable(Tvariable *v)
Destructor.
Definition: variable.c:93
void IntervalSubstract(Tinterval *i1, Tinterval *i2, Tinterval *i_out)
Substraction of two intervals.
Definition: interval.c:442
void InitVector(unsigned int ele_size, void(*Copy)(void *, void *), void(*Delete)(void *), unsigned int max_ele, Tvector *vector)
Constructor.
Definition: vector.c:100
Tlink * l
Definition: link.h:277
void HTransformScaleX(double s, THTransform *t)
Constructor.
Definition: htransform.c:249
void NewVariable(unsigned int type, char *name, Tvariable *v)
Constructor.
Definition: variable.c:20
void DeleteInterval(Tinterval *i)
Destructor.
Definition: interval.c:1021
void AddEquation2CS(Tparameters *p, Tequation *eq, TCuikSystem *cs)
Adds an equation to the system.
Definition: cuiksystem.c:2502
A scaled product of powers of variables.
Definition: monomial.h:32
A table of parameters.
double UpperLimit(Tinterval *i)
Gets the uppser limit.
Definition: interval.c:87
A sequence of transforms.
Definition: trans_seq.h:319
double GetPolyhedronMaxCoordinate(Tpolyhedron *p)
Returns the maximum coordinate value used in a polyhedron.
Definition: polyhedron.c:1458
#define CT_REPRESENTATION
Representation.
Definition: parameters.h:215
void AddVarTrans2TransSeq(unsigned int t, int s, unsigned int v, TTransSeq *ts)
Adds a variable transform to the sequence.
Definition: trans_seq.c:578
boolean ZeroInterval(Tinterval *i)
Checks if a given interval is zero.
Definition: interval.c:340
void GenerateScaledSaddleEquation(double s, unsigned int vx, unsigned int vy, unsigned int vz, Tequation *eq)
Construtor. Generates a scaled saddle equation.
Definition: equation.c:1461
#define SPHERE
One of the possible type of polyhedrons.
Definition: polyhedron.h:70
A box.
Definition: box.h:83
Data associated with each variable in the problem.
Definition: variable.h:84
unsigned int GetPolyhedronStatus(Tpolyhedron *p)
Gets the status of a polyhedron (NORMAL, HIDDEN, DECOR).
Definition: polyhedron.c:1378
Definition of the Tpolyhedron type and the associated functions.
void VarAccumulateEquations(Tequation *eqn, unsigned int v, Tequation *eq)
Adds an equation scaled with a variable to another equation.
Definition: equation.c:377
A cuiksystem, i.e., a set of variables and equations defining a position analysis problem...
Definition: cuiksystem.h:181
#define NO_UINT
Used to denote an identifier that has not been initialized.
Definition: defines.h:435
void MovePolyhedron(Tplot3d *pt, THTransform *t, Tpolyhedron *p)
Moves an object previously added to a 3D scene.
Definition: polyhedron.c:1517
void IntervalScale(Tinterval *i1, double e, Tinterval *i_out)
Scales an interval.
Definition: interval.c:360
void AddVariable2Monomial(unsigned int fn, unsigned int varid, unsigned int p, Tmonomial *f)
Adds a power variable to the monomial.
Definition: monomial.c:171
boolean IsSystemVarInSimpCS(Tparameters *p, char *v, TCuikSystem *cs)
Identifies system variables that survive in the simplified system.
Definition: cuiksystem.c:2641
void HTransformTx(double tx, THTransform *t)
Constructor.
Definition: htransform.c:112
double * p
Definition: link.h:279
void DeleteVoidPtr(void *a)
Destructor for void pointers.
Definition: vector.c:92
Tinterval * GetBoxIntervals(Tbox *b)
Returns a pointer to the array of intervals defining the box.
Definition: box.c:284
void HTransformDelete(THTransform *t)
Destructor.
Definition: htransform.c:887
double GetParameter(unsigned int n, Tparameters *p)
Gets the value for a particular parameter.
Definition: parameters.c:93
void DeletePolyhedron(Tpolyhedron *p)
Destructor.
Definition: polyhedron.c:1692
void IntervalProduct(Tinterval *i1, Tinterval *i2, Tinterval *i_out)
Product of two intervals.
Definition: interval.c:389
void DeleteColor(Tcolor *c)
Destructor.
Definition: color.c:143
void NewInterval(double lower, double upper, Tinterval *i)
Constructor.
Definition: interval.c:47
void HTransformSetElement(unsigned int i, unsigned int j, double v, THTransform *t)
Sets an element in a homogeneous transform.
Definition: htransform.c:312
#define INF
Infinite.
Definition: defines.h:70
void AddCt2Monomial(double k, Tmonomial *f)
Scales a monomial.
Definition: monomial.c:158
void HTransformCopy(THTransform *t_dst, THTransform *t_src)
Copy constructor.
Definition: htransform.c:83
void DefineNormalVector(double *v, double *n)
Defines a unitary vector normal to a given vector.
Definition: geom.c:571
unsigned int GetCSVariableID(char *name, TCuikSystem *cs)
Gets the numerical identifier of a variable given its name.
Definition: cuiksystem.c:2607
void NewColor(double r, double g, double b, Tcolor *c)
Constructor.
Definition: color.c:14
Defines a interval.
Definition: interval.h:33
#define LINK_MAX_FORCE(vname, linkName)
Max force variable.
Definition: varnames.h:119
void CopyPolyhedron(Tpolyhedron *p_dst, Tpolyhedron *p_src)
Copy constructor.
Definition: polyhedron.c:1259
void HTransformIdentity(THTransform *t)
Constructor.
Definition: htransform.c:69
void InitMonomial(Tmonomial *f)
Constructor.
Definition: monomial.c:17
double IntervalSize(Tinterval *i)
Gets the uppser limit.
Definition: interval.c:96
void CrossProduct(double *v1, double *v2, double *v3)
Computes the cross product of two 3d vectors.
Definition: geom.c:639
void PolyhedronPrintCenter(FILE *f, THTransform *t, Tpolyhedron *p)
Prints the center of a sphere to a file.
Definition: polyhedron.c:1493
unsigned int StartNew3dObject(Tcolor *c, Tplot3d *p)
Start a composed object.
Definition: plot3d.c:157
void HTransformOrthonormalize(THTransform *t, THTransform *ta)
Orthonormalizes the rotation part of a homogenouos transform.
Definition: htransform.c:529
#define DUMMY_VAR
One of the possible type of variables.
Definition: variable.h:53
#define LINK_TRANS(vname, linkName, cn)
Translation part of the homogeneous transform defining the position of a link in global coordinates...
Definition: varnames.h:168
#define LINK_ROT2(vname, linkName, vn, cn)
Frame of reference for a link.
Definition: varnames.h:86
void Close3dObject(Tplot3d *p)
Closes a composed object.
Definition: plot3d.c:171
void DeleteMonomial(Tmonomial *f)
Destructor.
Definition: monomial.c:289
unsigned int GetPolyhedronType(Tpolyhedron *p)
Retrives the type of a polyhedron.
Definition: polyhedron.c:1363
void GenerateDotProductEquation(unsigned int v1x, unsigned int v1y, unsigned int v1z, unsigned int v2x, unsigned int v2y, unsigned int v2z, unsigned int vc, double c, Tequation *eq)
Construtor. Generates the equation of the dot product of two unitary vectors.
Definition: equation.c:1588
unsigned int NewVectorElement(void *e, Tvector *vector)
Adds an element to the vector.
Definition: vector.c:216
#define DECOR_SHAPE
One of the possible type of shapes.
Definition: polyhedron.h:46
Link configuration.
Definition: link.h:276
#define HIDDEN_SHAPE
One of the possible type of shapes.
Definition: polyhedron.h:37
#define REP_QLINKS
One of the possible values of the REPRESENTATION parameter.
Definition: parameters.h:48