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 TransformManipulator::perform(const Ray * r)
{
if(isDetached()) return;
const Vector3F worldP = worldSpace().getTranslation();
Matrix44F ps;
parentSpace(ps);
Plane pl(ps.transformAsNormal(hitPlaneNormal()), worldP);
Vector3F hit, d;
float t;
if(pl.rayIntersect(*r, hit, t, 1)) {
d = hit - worldP;
if(d.length() > 10.f) return;
if(m_mode == ToolContext::RotateTransform) {
d = d.normal() * 8.f;
hit = worldP + d;
}
d = hit - m_currentPoint;
if(m_mode == ToolContext::MoveTransform)
move(d);
else
spin(d);
m_currentPoint = hit;
}
}
Matrix44F BaseTransform::space() const
{
Matrix44F s;
s.setTranslation(m_translation);
s.setRotation(rotation());
return s;
}
const Vector3F Chassis::torsionBarHinge(const int & i, bool isLeft) const
{
const Matrix44F mat = bogieArmOrigin(i, isLeft);
Vector3F p = mat.getTranslation();
p.z += 0.5f * m_bogieArmLength * cos(m_torsionBarRestAngle);
p.y += 0.5f * m_bogieArmLength * sin(m_torsionBarRestAngle);
return p;
}
void Matrix44F::rotateY(float beta)
{
const float c = cos(beta);
const float s = sin(beta);
Matrix44F r;
*r.m(0, 0) = c; *r.m(0, 2) = -s;
*r.m(2, 0) = s; *r.m(2, 2) = c;
multiply(r);
}
void Matrix44F::rotateZ(float gamma)
{
const float c = cos(gamma);
const float s = sin(gamma);
Matrix44F r;
*r.m(0, 0) = c; *r.m(0, 1) = s;
*r.m(1, 0) = -s; *r.m(1, 1) = c;
multiply(r);
}
void Matrix44F::rotateX(float alpha)
{
const float c = cos(alpha);
const float s = sin(alpha);
Matrix44F r;
*r.m(1, 1) = c; *r.m(1, 2) = s;
*r.m(2, 1) = -s; *r.m(2, 2) = c;
multiply(r);
}
Matrix44F BaseTransform::space() const
{
Matrix44F s;
s.setTranslation(m_translation);
Matrix33F r = orientation();
s.translate(m_rotatePivotTranslate);
s.translate(m_rotatePivot);
s.translate(r.transform(m_rotatePivot.reversed()));
s.translate(r.transform(m_scalePivotTranslate));
s.translate(r.transform(m_scalePivot));
Vector3F displaceByScaling = m_scalePivot.reversed();
displaceByScaling = displaceByScaling * m_scale;
s.translate(r.transform(displaceByScaling));
Matrix33F scaleMatrix;
*scaleMatrix.m(0, 0) = m_scale.x;
*scaleMatrix.m(1, 1) = m_scale.y;
*scaleMatrix.m(2, 2) = m_scale.z;
r = scaleMatrix * r;
s.setRotation(r);
return s;
}
const Matrix44F Chassis::computeBogieArmOrigin(const float & chassisWidth, const Vector3F & wheelP, const float & l, const float & s, const float & ang) const
{
Matrix44F tm;
tm.rotateX(-ang);
Vector3F p;
p.x = chassisWidth * .5f + s * .5f;
p.y = wheelP.y + l * .5f * sin(ang);
p.z = wheelP.z + l * .5f * cos(ang);
tm.setTranslation(p);
return tm;
}
void TransformManipulator::move(const Vector3F & d)
{
Matrix44F ps;
parentSpace(ps);
Matrix44F invps = ps;
invps.inverse();
Vector3F od = invps.transformAsNormal(d);
m_subject->translate(od);
setTranslation(m_subject->translation());
}
const Matrix44F Matrix44F::transformBy(const Matrix44F & a) const
{
Matrix44F t;
int i, j;
for(i = 0; i < 4; i++) {
for(j = 0; j < 4; j++) {
*t.m(i, j) = M(i, 0) * a.M(0, j) + M(i, 1) * a.M(1, j) + M(i, 2) * a.M(2, j) + M(i, 3) * a.M(3, j);
}
}
return t;
}
const Matrix44F Chassis::bogieArmOrigin(const int & i, bool isLeft) const
{
Matrix44F res;
res.rotateX(-m_torsionBarRestAngle);
Vector3F cen = roadWheelOrigin(i, isLeft);
float d = 1.f;
if(!isLeft) d = -d;
cen.x += -m_trackWidth * .5f * d + m_bogieArmWidth * .7f * d;
cen.z += 0.5f * m_bogieArmLength * cos(m_torsionBarRestAngle);
cen.y += 0.5f * m_bogieArmLength * sin(m_torsionBarRestAngle);
res.setTranslation(cen);
return res;
}
Vector3F SkeletonSubspaceDeformer::combine(unsigned idx)
{
const unsigned nj = numBindJoints(idx);
unsigned j;
Vector3F q;
Matrix44F space;
for(j = 0; j < nj; j++) {
space = bindS(idx, j);
q += space.transform(bindP(idx, j)) * bindW(idx, j);
}
return q;
}
bool BaseTransform::intersect(const Ray & ray) const
{
Matrix44F s = worldSpace();
s.inverse();
Vector3F a = ray.m_origin + ray.m_dir * ray.m_tmin;
a = s.transform(a);
Vector3F b = ray.m_origin + ray.m_dir * ray.m_tmax;
b = s.transform(b);
Ray objR(a, b);
float hit0, hit1;
return getBBox().intersect(objR, &hit0, &hit1);
}
void BaseView::frameAll(const BoundingBox & b)
{
Vector3F eye = b.center();
eye.z = b.getMax(2) + b.distance(0) / hfov() * .55f + 120.f;
setEyePosition(eye);
Matrix44F m;
m.setTranslation(eye);
*cameraSpaceR() = m;
m.inverse();
*cameraInvSpaceR() = m;
setFrustum(1.33f, 1.f, 26.2f, -1.f, -1000.f);
}
void BaseTransform::parentSpace(Matrix44F & dst) const
{
if(!parent()) return;
BaseTransform * p = parent();
while(p) {
dst.multiply(p->space());
p = p->parent();
}
}
void SkeletonSubspaceDeformer::calculateSubspaceP()
{
unsigned i, j, n, nj, vstart;
Matrix44F spaceInv;
Vector3F p;
for(i = 0; i < numVertices(); i++) {
p = getDeformedP()[i];
n = m_jointIds[i]._ndim;
nj = n - 1;
vstart = m_jointIds[i][nj];
for(j = 0; j < nj; j++) {
SkeletonJoint * joint = m_skeleton->jointByIndex(m_jointIds[i][j]);
spaceInv = joint->worldSpace();
spaceInv.inverse();
m_subspaceP[vstart + j] = spaceInv.transform(p);
}
}
}
void TransformManipulator::start(const Ray * r)
{
const Vector3F worldP = worldSpace().getTranslation();
Matrix44F ps;
parentSpace(ps);
Plane pl(ps.transformAsNormal(hitPlaneNormal()), worldP);
Vector3F hit, d;
float t;
if(pl.rayIntersect(*r, hit, t, 1)) {
d = hit - worldP;
if(d.length() > 10.f) return;
if(m_mode == ToolContext::RotateTransform) {
d = d.normal() * 8.f;
hit = worldP + d;
}
m_startPoint = hit;
m_currentPoint = m_startPoint;
}
m_started = 1;
}
void MlUVView::drawControlVectors(MlFeather * f)
{
Vector3F baseP(f->baseUV());
GeoDrawer * dr = getDrawer();
glPushMatrix();
Matrix44F s;
s.setTranslation(baseP);
float * quill = f->getQuilly();
Vector3F b, a;
Vector2F *d;
for(short i=0; i <= f->numSegment(); i++) {
dr->useSpace(s);
a.setZero();
d = f->uvDisplaceAt(i, 0);
for(short j = 0; j < 3; j++) {
b = d[j];
dr->arrow(a, a + b);
a += b;
}
a.setZero();
d = f->uvDisplaceAt(i, 1);
for(short j = 0; j < 3; j++) {
b = d[j];
dr->arrow(a, a + b);
a += b;
}
a.setZero();
b.set(0.f, quill[i], 0.f);
if(i<f->numSegment()) dr->arrow(a, b);
s.setTranslation(b);
}
glPopMatrix();
}
void Matrix44F::multiply(const Matrix44F & a)
{
Matrix44F t(*this);
setZero();
int i, j, k;
for(i = 0; i < 4; i++) {
for(j = 0; j < 4; j++) {
for(k = 0; k < 4; k++) {
*m(i, j) += t.M(i, k) * a.M(k, j);
}
}
}
}
void MlUVView::drawBindVectors(MlFeather * f)
{
GeoDrawer * dr = getDrawer();
const Vector3F baseP(f->baseUV());
glPushMatrix();
Matrix44F s;
s.setTranslation(baseP);
float * quill = f->getQuilly();
Vector3F b;
for(short i=0; i < f->numSegment(); i++) {
dr->useSpace(s);
for(short j=0; j < f->numBind(i); j++) {
DeformableFeather::BindCoord *bind = f->getBind(i, j);
dr->arrow(Vector3F(0.f, 0.f, 0.f), bind->_objP * 32.f);
}
b.set(0.f, quill[i], 0.f);
s.setTranslation(b);
}
glPopMatrix();
}
void ProxyViz::draw( M3dView & view, const MDagPath & path,
M3dView::DisplayStyle style,
M3dView::DisplayStatus status )
{
if(!m_enableCompute) return;
MObject thisNode = thisMObject();
updateWorldSpace(thisNode);
MPlug mutxplug( thisNode, axmultiplier);
MPlug mutyplug( thisNode, aymultiplier);
MPlug mutzplug( thisNode, azmultiplier);
setScaleMuliplier(mutxplug.asFloat(),
mutyplug.asFloat(),
mutzplug.asFloat() );
MPlug svtPlug(thisNode, adisplayVox);
setShowVoxLodThresold(svtPlug.asFloat() );
MDagPath cameraPath;
view.getCamera(cameraPath);
if(hasView() ) updateViewFrustum(thisNode);
else updateViewFrustum(cameraPath);
setViewportAspect(view.portWidth(), view.portHeight() );
MPlug actp(thisNode, aactivated);
if(actp.asBool()) setWireColor(.125f, .1925f, .1725f);
else setWireColor(.0675f, .0675f, .0675f);
_viewport = view;
fHasView = 1;
view.beginGL();
double mm[16];
matrix_as_array(_worldInverseSpace, mm);
glPushMatrix();
glMultMatrixd(mm);
ExampVox * defBox = plantExample(0);
updateGeomBox(defBox, thisNode);
drawWireBox(defBox->geomCenterV(), defBox->geomScale() );
Matrix44F mat;
mat.setFrontOrientation(Vector3F::YAxis);
mat.scaleBy(defBox->geomSize() );
mat.glMatrix(m_transBuf);
drawCircle(m_transBuf);
drawGridBounding();
// drawGrid();
if ( style == M3dView::kFlatShaded ||
style == M3dView::kGouraudShaded ) {
drawPlants();
}
else
drawWiredPlants();
if(hasView() ) drawViewFrustum();
drawBrush(view);
drawActivePlants();
drawGround();
glPopMatrix();
view.endGL();
std::cout<<" viz node draw end";
}
