void reorient(){
if(A[X] == 0){
rotateForward();
} else if(A[X] == dimensions[L]){
rotateForward();
rotateForward();
rotateForward();
} else if(A[Y] == 0){
rotateRight();
} else if(A[Y] == dimensions[W]){
rotateRight();
rotateRight();
rotateRight();
} else if(A[Z] == 0){
rotateRight();
rotateRight();
}
}
/* Suspend the current running process, and launch a running process. May be the
* same if no other process are currently running*/
void yield(void)
{
LOG("Transfering processes (current, next) : ");
struct processDescriptor* current =
(struct processDescriptor*) getIndex(&runningList, 0)->element;
rotateForward(&runningList);
struct processDescriptor* next =
(struct processDescriptor*) getIndex(&runningList, 0)->element;
LOG_INT(current->pid);
LOG_CONT(" ");
LOG_INT(next->pid);
LOG_CONT("\n");
wait(1000000);
transfer(&(next->processState), &(current->processState));
}
void MlRachis::bend(unsigned faceIdx, float patchU, float patchV, const Vector3F & oriP, const Matrix33F & space, float radius, CollisionRegion * collide)
{
reset();
Matrix33F invSpace, segSpace = space;
Vector3F pop, topop, segU, smoothU, testP, toTestP, segP = oriP;
Vector2F rotAngles;
float segRot, pushAngle, curAngle, smoothAngle, segL, dL, bendWei;
invSpace = segSpace;
invSpace.inverse();
const Vector3F rootUp = segSpace.transform(Vector3F::XAxis);
const Vector3F rootFront = segSpace.transform(Vector3F::ZAxis);
pushAngle = 0.f;
testP = segP;
segL = radius * m_lengthPortions[0];
moveForward(segSpace, segL, testP);
segU = collide->getClosestNormal(testP, 1000.f, pop);
topop = pop - segP;
pushAngle = pushToSurface(topop, invSpace);
collide->interpolateVertexVector(faceIdx, patchU, patchV, &smoothU);
Float3 rota = matchNormal(smoothU, invSpace);
smoothAngle = rota.x;
curAngle = pushAngle + smoothAngle;
m_spaces[0].rotateZ(rota.y);
m_spaces[0].rotateY(curAngle);
m_angles[0].x = curAngle;
m_angles[0].y = rota.y;
rotateForward(m_spaces[0], segSpace);
moveForward(segSpace, segL, segP);
for(unsigned i = 1; i < m_numSpace; i++) {
invSpace = segSpace;
invSpace.inverse();
testP = segP;
segL = radius * m_lengthPortions[i];
moveForward(segSpace, segL, testP);
segU = collide->getClosestNormal(testP, 1000.f, pop);
dL = Vector3F(testP, pop).length();
bendWei = dL/segL;
if(bendWei > 1.f) bendWei = 1.f;
toTestP = testP - segP;
topop = pop - segP;
pushAngle = 0.f;
if(toTestP.dot(topop) > 0.f) pushAngle = pushToSurface(topop, invSpace);
else bendWei = 0.f;
collide->interpolateVertexVector(&segU);
rota = matchNormal(segU, invSpace);
segRot = rota.x;
if(segRot > 0.f) segRot *= .5f;
curAngle = pushAngle;
curAngle += segRot * bendWei;
curAngle += 0.2f * m_lengths[i];
m_spaces[i].rotateZ(rota.y);
m_spaces[i].rotateY(curAngle);
m_angles[i].x = curAngle;
m_angles[i].y = rota.y;
rotateForward(m_spaces[i], segSpace);
moveForward(segSpace, segL, segP);
}
m_bendDirection = -rootUp.angleBetween(segU, rootFront);
}
