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;
}
Vector3F BaseTransform::rotatePlane(RotateAxis a) const
{
Matrix33F base;
base.rotateEuler(rotationBaseAngles().x, rotationBaseAngles().y, rotationBaseAngles().z);
if(a == AZ) return base.transform(Vector3F::ZAxis);
Matrix33F r;
if(a == AY) {
r.rotateEuler(0.f, 0.f, rotationAngles().z);
r.multiply(base);
return r.transform(Vector3F::YAxis);
}
r = orientation();
return r.transform(Vector3F::XAxis);
}
void BlockBccMeshBuilder::addAnchorByThreshold(ATetrahedronMesh * mesh,
unsigned istripe,
const Matrix33F & invspace,
const Vector3F & center,
float threshold,
bool isLower,
unsigned tri)
{
unsigned tetMax = mesh->numTetrahedrons() * 4;
if(istripe < indexDrifts.size() -1)
tetMax = indexDrifts[istripe + 1];
BoundingBox box;
Vector3F q;
unsigned i = indexDrifts[istripe];
unsigned j;
for(;i<tetMax;i+=4) {
unsigned * tet = mesh->tetrahedronIndices(i/4);
box.reset();
for(j=0; j< 4; j++)
box.expandBy(mesh->points()[tet[j]], 1e-3f);
q = invspace.transform(box.center() - center);
if(isLower) {
if(q.x > threshold) continue;
}
else {
if(q.x < threshold) continue;
}
for(j=0; j< 4; j++)
mesh->anchors()[tet[j]] = (1<<24 | tri);
}
}
float MlRachis::pushToSurface(const Vector3F & wv, const Matrix33F & space)
{
Vector3F ov = space.transform(wv);
ov.normalize();
ov.y = 0.f;
ov.x += 0.05f;
ov.normalize();
float a = acos(Vector3F::ZAxis.dot(ov));
if(ov.x < 0.f) a = 0.f;
return a;
}
Float3 MlRachis::matchNormal(const Vector3F & wv, const Matrix33F & space)
{
Vector3F ov = space.transform(wv);
Vector3F va = ov;
va.y = 0.f;
va.z -= 0.05f;
va.normalize();
float a = acos(Vector3F::XAxis.dot(va));
if(va.z > 0.f) a = -a;
Vector3F vb = ov;
vb.z = 0.f;
vb.normalize();
float b = acos(Vector3F::XAxis.dot(vb));
if(vb.y < 0.f) b = -b;
return Float3(a, b, 0.f);
}
void MlRachis::moveForward(const Matrix33F & space, float distance, Vector3F & dst)
{
Vector3F wv = space.transform(Vector3F::ZAxis);
wv.normalize();
dst += wv * distance;
}