void VPathRenderingData::RemoveLinkRoll(hkvMat3 const& mPrevRot, hkvMat3& mRot, bool& bDirection) const
{
hkvMat3 mPrevRotInv(mPrevRot);
VVERIFY(mPrevRotInv.invert() == HKV_SUCCESS);
// Calculate the rotation of the current link in the space of the
// previous link
hkvMat3 mThisInPrev = mPrevRotInv.multiply(mRot);
// Check whether we are changing direction (that is, we have a sharp angle
// between two links). In this case, we need to change how we adjust the roll.
hkvVec3 v2(mThisInPrev * hkvVec3(1, 0, 0));
bool bChangingDirection = bDirection ? v2[0] > 0 : v2[0] < 0;
if (bChangingDirection)
bDirection = !bDirection;
// Adjust the roll component
float rgfThisInPrev[3];
mThisInPrev.getAsEulerAngles(rgfThisInPrev[2], rgfThisInPrev[1], rgfThisInPrev[0]);
if (bDirection)
rgfThisInPrev[2] = 180.f;
else
rgfThisInPrev[2] = 0.f;
mRot.setFromEulerAngles(rgfThisInPrev[2], rgfThisInPrev[1], rgfThisInPrev[0]);
// Transform back to global space
mRot = mPrevRot.multiply(mRot);
}
///
/// \brief
/// Converts a Havok Physics rotation matrix to a Vision rotation matrix.
///
VHAVOK_IMPEXP static HK_FORCE_INLINE void HkRotationToVisMatrix(const hkRotation &hkRot, hkvMat3 &visRotMatrixOut)
{
// Extract matrix columns from Havok, both col-major
float* visRot = (float*)visRotMatrixOut.getPointer();
hkRot.getColumn<0>().store<3,HK_IO_NATIVE_ALIGNED>(visRot );
hkRot.getColumn<1>().store<3,HK_IO_NATIVE_ALIGNED>(visRot+3);
hkRot.getColumn<2>().store<3,HK_IO_NATIVE_ALIGNED>(visRot+6);
}
inline void VGroupInstance::Read(IVFileInStream *pIn, AvailableFlags_e eFlags)
{
if ((eFlags&INSTANCE_POSITION)>0) pIn->Read(&m_vPosition,sizeof(m_vPosition),"fff");
if ((eFlags&INSTANCE_ROTATION)>0) pIn->Read(m_Rotation.getPointer(),sizeof(m_Rotation),"9f");
if ((eFlags&INSTANCE_SCALE)>0) pIn->Read(&m_fScale,sizeof(m_fScale),"f");
if ((eFlags&INSTANCE_COLOR)>0) {pIn->Read(&m_iColor.r,1);pIn->Read(&m_iColor.g,1);pIn->Read(&m_iColor.b,1);pIn->Read(&m_iColor.a,1);}
if ((eFlags&INSTANCE_ANIMATION)>0) {pIn->Read(&m_iAnimationIndex,sizeof(m_iAnimationIndex),"s");pIn->Read(&m_fAnimationOffset,sizeof(m_fAnimationOffset),"f");}
if ((eFlags&INSTANCE_FARCLIP)>0) pIn->Read(&m_fFarClip,sizeof(m_fFarClip),"f");
if ((eFlags&INSTANCE_SEED)>0) pIn->Read(&m_fSeed,sizeof(m_fSeed),"f");
if ((eFlags&INSTANCE_SPLITKEY)>0) pIn->Read(&m_iSplitKey,sizeof(m_iSplitKey),"i");
if ((eFlags&INSTANCE_CONEANGLE)>0) {pIn->Read(&m_fConeAngleX,sizeof(m_fConeAngleX),"f");pIn->Read(&m_fConeAngleY,sizeof(m_fConeAngleY),"f");}
}
///
/// \brief
/// Converts a Vision rotation matrix to a Havok Physics rotation matrix.
///
VHAVOK_IMPEXP static HK_FORCE_INLINE void VisMatrixToHkRotation(const hkvMat3 &visRotMatrix, hkRotation &hkRotOut)
{
// Extract matrix columns from Vision
const float* visRot = (const float*)visRotMatrix.getPointer();
hkRotOut.getColumn<0>().load<3,HK_IO_NATIVE_ALIGNED>(visRot );
hkRotOut.getColumn<0>().zeroComponent<3>();
hkRotOut.getColumn<1>().load<3,HK_IO_NATIVE_ALIGNED>(visRot+3);
hkRotOut.getColumn<1>().zeroComponent<3>();
hkRotOut.getColumn<2>().load<3,HK_IO_NATIVE_ALIGNED>(visRot+6);
hkRotOut.getColumn<2>().zeroComponent<3>();
HK_ASSERT(0xdee887, hkRotOut.isOk() /*&& hkRotOut.isOrthonormal()*/);
}
void VImageState::CreateTransformation(hkvMat3 &dest, const hkvVec2 &vOfs, float fRotation, float fScaling)
{
dest.setIdentity();
float sw = fScaling * hkvMath::sinDeg (fRotation);
float cw = fScaling * hkvMath::cosDeg (fRotation);
// upper 2x2 rotation part
dest.m_Column[0][0] = cw; dest.m_Column[1][0] = -sw;
dest.m_Column[0][1] = sw; dest.m_Column[1][1] = cw;
// translation column
dest.m_Column[2][0] = vOfs.x;
dest.m_Column[2][1] = vOfs.y;
}
BOOL VWallmarkManager::TryAlignWallmark(
const hkvVec3& vCenter,
const hkvVec3& vNormal,
float fSize, float fRotation,
hkvVec3& vNewCenter,
hkvMat3 &alignment,
float fEpsilon
)
{
VISION_PROFILE_FUNCTION(PROFILING_WALLMARK_CREATION);
hkvVec3 vNewNormal(hkvNoInitialization);
float fTraceRad = fSize;
if (!IsTracePointOnPlane(vCenter,vNormal,fTraceRad,fEpsilon,vNewNormal))
return false;
hkvVec3 vRight(hkvNoInitialization),vUp(hkvNoInitialization),vRotRight(hkvNoInitialization),vRotUp(hkvNoInitialization), vDummy(hkvNoInitialization);
if (hkvMath::Abs (vNewNormal.x)>0.5f)
vRight.set(0.f,1.f,0.f);
else
vRight.set(1.f,0.f,0.f);
vUp = vNewNormal.cross(vRight);
vRight = vNewNormal.cross(vUp);
float fSin = hkvMath::sinDeg (fRotation);
float fCos = hkvMath::cosDeg (fRotation);
vRotRight = vRight * fCos + vUp * fSin;
vRotUp = vRight * -fSin + vUp * fCos;
vRotRight.setLength(fSize*0.5f);
vRotUp.setLength(fSize*0.5f);
alignment.setAxisXYZ (vNewNormal,vRotRight,vRotUp);
vNewCenter = vCenter + vNewNormal*fEpsilon;
// check corners:
if (!IsTracePointOnPlane(vCenter+vRotRight+vRotUp,vNewNormal,fTraceRad,fEpsilon,vDummy))
return false;
if (!IsTracePointOnPlane(vCenter+vRotRight-vRotUp,vNewNormal,fTraceRad,fEpsilon,vDummy))
return false;
if (!IsTracePointOnPlane(vCenter-vRotRight+vRotUp,vNewNormal,fTraceRad,fEpsilon,vDummy))
return false;
if (!IsTracePointOnPlane(vCenter-vRotRight-vRotUp,vNewNormal,fTraceRad,fEpsilon,vDummy))
return false;
return TRUE;
}