void hsMatrix44::MakeCameraMatrices(const hsPoint3& from, const hsPoint3& at, const hsVector3& up, hsMatrix44& worldToCamera, hsMatrix44& cameraToWorld)
{
hsVector3 dirZ(&at, &from);
hsVector3 dirX(dirZ % up);
dirX.Normalize();
hsVector3 dirY(dirX % dirZ);
dirY.Normalize();
worldToCamera.Reset(false);
cameraToWorld.Reset(false);
int i;
for( i = 0; i < 3; i++ )
{
worldToCamera.fMap[0][i] = dirX[i];
worldToCamera.fMap[1][i] = dirY[i];
worldToCamera.fMap[2][i] = dirZ[i];
cameraToWorld.fMap[i][0] = dirX[i];
cameraToWorld.fMap[i][1] = dirY[i];
cameraToWorld.fMap[i][2] = dirZ[i];
}
hsPoint3 trans = -from;
worldToCamera.fMap[0][3] = dirX.InnerProduct(trans);
worldToCamera.fMap[1][3] = dirY.InnerProduct(trans);
worldToCamera.fMap[2][3] = dirZ.InnerProduct(trans);
cameraToWorld.fMap[0][3] = from.fX;
cameraToWorld.fMap[1][3] = from.fY;
cameraToWorld.fMap[2][3] = from.fZ;
}
//
// Camera matrix
//
hsMatrix44& hsMatrix44::MakeCameraUpPreserving(const hsPoint3* from, const hsPoint3* at,
const hsVector3* up)
{
hsVector3 dirZ(at, from);
hsVector3 trans( -from->fX, -from->fY, -from->fZ );
hsVector3 dirY( up->fX, up->fY, up->fZ );
hsVector3 dirX;
hsMatrix44 rmat;
dirX = dirY % dirZ;
dirX.Normalize();
dirY.Normalize();
dirZ = dirX % dirY;
dirZ.Normalize();
this->Reset();
this->Translate(&trans);
rmat.Reset();
for(int i = 0; i < 3; i++)
{
rmat.fMap[0][i] = -dirX[i];
rmat.fMap[1][i] = dirY[i];
rmat.fMap[2][i] = dirZ[i];
}
rmat.NotIdentity();
*this = rmat * *this;
return *this;
}
//
// Camera matrix
//
hsMatrix44& hsMatrix44::MakeCamera(const hsPoint3* from, const hsPoint3* at,
const hsVector3* up)
{
hsVector3 dirZ(at, from);
hsVector3 trans( -from->fX, -from->fY, -from->fZ );
hsVector3 dirY, dirX;
hsMatrix44 rmat;
dirX = (*up) % dirZ; // Stop passing in down!!! // mf_flip_up - mf
if (dirX.MagnitudeSquared())
dirX.Normalize();
if (dirZ.MagnitudeSquared())
dirZ.Normalize();
dirY = dirZ % dirX;
if (dirY.MagnitudeSquared())
dirY.Normalize();
this->Reset();
this->Translate(&trans);
rmat.Reset();
for(int i = 0; i < 3; i++)
{
rmat.fMap[0][i] = -dirX[i];
rmat.fMap[1][i] = dirY[i];
rmat.fMap[2][i] = dirZ[i];
}
rmat.NotIdentity();
*this = rmat * *this;
return *this;
}
bool ContactDistanceCalc<BasicTraits,BVOL,Metric>::InitDouble
(GroupType* g0, const TransformType& m0, const TransformType& f0,
GroupType* g1, const TransformType& m1, const TransformType& f1)
{
// transformation matrix model1 to model0
// m0^(-1) * m1
if (!m_M1ToM0.invertFrom(m0)) {
return false;
}
m_M1ToM0.mult(m1);
// m1^(-1) * m0
m_M0ToM1.invertFrom(m_M1ToM0);
m_M0 = m0;
u32 k, kk;
VectorClass trans(m_M1ToM0[3][0], m_M1ToM0[3][1], m_M1ToM0[3][2]);
VectorClass dirX(m_M1ToM0[0][0], m_M1ToM0[0][1], m_M1ToM0[0][2]);
VectorClass dirY(m_M1ToM0[1][0], m_M1ToM0[1][1], m_M1ToM0[1][2]);
VectorClass dirZ(m_M1ToM0[2][0], m_M1ToM0[2][1], m_M1ToM0[2][2]);
for (k=0; k<BVOL::Size; ++k) {
m_proj[k][0] = BVOL::getDirection()[k].dot(dirX);
m_proj[k][1] = BVOL::getDirection()[k].dot(dirY);
m_proj[k][2] = BVOL::getDirection()[k].dot(dirZ);
m_M1Offset[k] = BVOL::getDirection()[k].dot(trans);
}
#if 0
for (k=0, kk=BVOL::Size; k<BVOL::Size; ++k, ++kk) {
m_M1ToM0.mult(BVOL::getDirection()[k], m_M1Direction[k]);
m_M1Direction[kk] = -m_M1Direction[k];
}
#endif
m_scale = m_proj[0].length();
u32 i;
Real value, maxValue, sx, sy;
for (k=0, kk=BVOL::Size; k<BVOL::Size; ++k, ++kk) {
// calc remapping m_perm
m_perm[k] = 0;
maxValue = m_proj[k].dot(BVOL::getDirection()[0]);
for (i=1; i<2*BVOL::Size; ++i) {
if ((value=m_proj[k].dot(BVOL::getDirection()[i])) > maxValue) {
maxValue = value;
m_perm[k] = i;
}
}
if (m_perm[k] < BVOL::Size) {
m_perm[kk] = m_perm[k]+BVOL::Size;
} else {
m_perm[kk] = m_perm[k]-BVOL::Size;
}
maxValue /= m_scale;
// mask
for (m_mask[k]=0; m_mask[k]<OccTableHighestBit; ++m_mask[k]) {
if (maxValue >= BVOL::unitDopAngleTable()[m_mask[k]]) {
break;
}
}
m_mask[kk] = m_mask[k];
#ifdef GV_WITH_SUBCONES
// calc ring section index m_submask[k],m_submask[kk]
dirZ = BVOL::getDirection()[m_perm[k]]-(maxValue*m_scale)*m_proj[k];
m_M1ToM0.mult(BVOL::BVolGeometry::getFrameX()[m_perm[k]], dirX);
//dirX -= dirX.dot(BVOL::getDirection()[k])*M1Direction[m_perm[k]];
//dirX = BVOL::BVolGeometry::getFrameX()[k];
m_M1ToM0.mult(BVOL::BVolGeometry::getFrameY()[m_perm[k]], dirY);
//dirY -= dirY.dot(BVOL::getDirection()[k])*M1Direction[m_perm[k]];
//dirY = BVOL::BVolGeometry::getFrameY()[k];
sx = dirZ.dot(dirX)/(dirZ.length()*dirX.length());
sy = dirZ.dot(dirY)/(dirZ.length()*dirY.length());
//std::cout << "k=" << k << ": (" << sx << ", " << sy << ")" << std::endl;
m_submask[k] = (sx < 0 ? 2 : 0)
+ (sx < 0 && sy >= 0 ? 1 : 0)
+ (sx >= 0 && sy < 0 ? 1 : 0);
assert(m_submask[k] < 4);
if (m_submask[k] < 2) {
m_submask[kk] = m_submask[k]+2;
} else {
m_submask[kk] = m_submask[k]-2;
}
assert(m_submask[kk] < 4);
#endif
}
m_intersect = false;
return true;
}
