int addTreeNode(n **ref, q *list, int data) {
n *nw = (n *)malloc(1*sizeof(n));
nw->data = data;
nw->leftc = NULL;
nw->rightc = NULL;
nw->parent = NULL;
printf("%x\t", nw);
if(list->front == list->rear) {
*ref = nw;
Qadd(list, nw);
return 1;
}
else {
n *parent = Qseek(list);
if(parent->leftc == NULL) {
parent->leftc = nw;
nw->parent = parent;
Qadd(list, nw);
return 1;
}
else if(parent->rightc == NULL) {
parent->rightc = nw;
nw->parent = parent;
Qadd(list, nw);
Qremove(list);
return 1;
}
}
}
ChQuaternion<double> AngleDTDT_to_QuatDTDT(AngleSet angset,
const ChVector<double>& mangles,
const ChQuaternion<double>& q) {
ChQuaternion<double> res;
ChQuaternion<double> qa, qb;
ChVector<double> ang0, angA, angB;
double hsquared = CH_LOWTOL;
ang0 = Quat_to_Angle(angset, q);
angA = Vsub(ang0, Vmul(mangles, hsquared));
angB = Vadd(ang0, Vmul(mangles, hsquared));
qa = Angle_to_Quat(angset, angA);
qb = Angle_to_Quat(angset, angB);
res = Qscale(Qadd(Qadd(qa, qb), Qscale(q, -2)), 1 / hsquared);
return res;
}
