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); }