void TransformManipulator::spin(const Vector3F & d)
{
Matrix44F ps;
parentSpace(ps);
Matrix44F invps = ps;
invps.inverse();
const Vector3F worldP = ps.transform(translation());
const Vector3F rotUp = ps.transformAsNormal(hitPlaneNormal());
Vector3F toa = m_currentPoint - worldP;
Vector3F tob = toa + d;
toa.normalize();
tob.normalize();
float ang = toa.angleBetween(tob, toa.cross(rotUp).reversed());
Vector3F angles;
if(m_rotateAxis == AY) angles.set(0.f, ang, 0.f);
else if(m_rotateAxis == AZ) angles.set(0.f, 0.f, ang);
else angles.set(ang, 0.f, 0.f);
m_subject->rotate(angles);
setRotationAngles(m_subject->rotationAngles());
}
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);
}