void Transform::Rotate(const Kiwi::Quaternion& rotation)
{
//multiply the current rotation with the new rotation to get the final rotation
m_rotation = rotation.Cross(m_rotation);
//if an axis is locked, we need to rotate that axis back to its fixed rotation value
if(m_lockRoll)
{
Kiwi::Vector3 eulerAngles = m_rotation.GetEulerAngles();
if(eulerAngles.z != m_lockPosition.z)
{
Kiwi::Quaternion zRot(this->GetForward(),-(eulerAngles.z - m_lockPosition.z));
m_rotation = zRot.Cross(m_rotation);
}
}
if(m_lockPitch)
{
Kiwi::Vector3 eulerAngles = m_rotation.GetEulerAngles();
if(eulerAngles.x != m_lockPosition.x)
{
Kiwi::Quaternion xRot(this->GetRight(),-(eulerAngles.x - m_lockPosition.x));
m_rotation = xRot.Cross(m_rotation);
}
}
if(m_lockYaw)
{
Kiwi::Vector3 eulerAngles = m_rotation.GetEulerAngles();
if(eulerAngles.y != m_lockPosition.y)
{
Kiwi::Quaternion yRot(this->GetUp(),-(eulerAngles.y - m_lockPosition.y));
m_rotation = yRot.Cross(m_rotation);
}
}
}
// Main application
int main( int argc, char* argv[] )
{
printf("== NaturalPoint Tracking Tools API Marker Sample =======---\n");
printf("== (C) NaturalPoint, Inc.\n\n");
printf("Initializing NaturalPoint Devices\n");
TT_Initialize();
// Do an update to pick up any recently-arrived cameras.
TT_Update();
// Load a project file from the executable directory.
printf( "Loading Project: project.ttp\n\n" );
CheckResult( TT_LoadProject("project.ttp") );
// List all detected cameras.
printf( "Cameras:\n" );
for( int i = 0; i < TT_CameraCount(); i++)
{
printf( "\t%s\n", TT_CameraName(i) );
}
printf("\n");
// List all defined rigid bodies.
printf("Rigid Bodies:\n");
for( int i = 0; i < TT_TrackableCount(); i++)
{
printf("\t%s\n", TT_TrackableName(i));
}
printf("\n");
int frameCounter = 0;
// Poll API data until the user hits a keyboard key.
while( !_kbhit() )
{
if( TT_Update() == NPRESULT_SUCCESS )
{
frameCounter++;
// Update tracking information every 100 frames (for example purposes).
if( (frameCounter%100) == 0 )
{
float yaw,pitch,roll;
float x,y,z;
float qx,qy,qz,qw;
bool tracked;
printf( "Frame #%d: (Markers: %d)\n", frameCounter, TT_FrameMarkerCount() );
for( int i = 0; i < TT_TrackableCount(); i++ )
{
TT_TrackableLocation( i, &x,&y,&z, &qx,&qy,&qz,&qw, &yaw,&pitch,&roll );
if( TT_IsTrackableTracked( i ) )
{
printf( "\t%s: Pos (%.3f, %.3f, %.3f) Orient (%.1f, %.1f, %.1f)\n", TT_TrackableName( i ),
x, y, z, yaw, pitch, roll );
TransformMatrix xRot( TransformMatrix::RotateX( -roll * kRadToDeg ) );
TransformMatrix yRot( TransformMatrix::RotateY( -yaw * kRadToDeg ) );
TransformMatrix zRot( TransformMatrix::RotateZ( -pitch * kRadToDeg ) );
// Compose the local-to-world rotation matrix in XZY (roll, pitch, yaw) order.
TransformMatrix worldTransform = xRot * zRot * yRot;
// Inject world-space coordinates of the origin.
worldTransform.SetTranslation( x, y, z );
// Invert the transform matrix to convert from a local-to-world to a world-to-local.
worldTransform.Invert();
float mx, my, mz;
int markerCount = TT_TrackableMarkerCount( i );
for( int j = 0; j < markerCount; ++j )
{
// Get the world-space coordinates of each rigid body marker.
TT_TrackablePointCloudMarker( i, j, tracked, mx, my, mz );
// Transform the rigid body point from world coordinates to local rigid body coordinates.
// Any world-space point can be substituted here to transform it into the local space of
// the rigid body.
Point4 worldPnt( mx, my, mz, 1.0f );
Point4 localPnt = worldTransform * worldPnt;
printf( "\t\t%d: World (%.3f, %.3f, %.3f) Local (%.3f, %.3f, %.3f)\n", j + 1,
mx, my, mz, localPnt[0], localPnt[1], localPnt[2] );
}
}
else
{
printf( "\t%s: Not Tracked\n", TT_TrackableName( i ) );
}
}
}
}
Sleep(2);
}
printf( "Shutting down NaturalPoint Tracking Tools\n" );
CheckResult( TT_Shutdown() );
printf( "Complete\n" );
while( !_kbhit() )
{
Sleep(20);
}
TT_FinalCleanup();
return 0;
}
/* static */
GfMatrix4d
UsdGeomXformOp::GetOpTransform(UsdGeomXformOp::Type const opType,
VtValue const &opVal,
bool isInverseOp)
{
// This will be the most common case.
if (opType == TypeTransform) {
GfMatrix4d mat(1.);
bool isMatrixVal = true;
if (opVal.IsHolding<GfMatrix4d>()) {
mat = opVal.UncheckedGet<GfMatrix4d>();
} else if (opVal.IsHolding<GfMatrix4f>()) {
mat = GfMatrix4d(opVal.UncheckedGet<GfMatrix4f>());
} else {
isMatrixVal = false;
TF_CODING_ERROR("Invalid combination of opType (%s) "
"and opVal (%s). Returning identity matrix.",
TfEnum::GetName(opType).c_str(),
TfStringify(opVal).c_str());
return GfMatrix4d(1.);
}
if (isMatrixVal && isInverseOp) {
double determinant=0;
mat = mat.GetInverse(&determinant);
if (GfIsClose(determinant, 0.0, 1e-9)) {
TF_CODING_ERROR("Cannot invert singular transform op with "
"value %s.", TfStringify(opVal).c_str());
}
}
return mat;
}
double doubleVal = 0.;
bool isScalarVal = true;
if (opVal.IsHolding<double>()) {
doubleVal = opVal.UncheckedGet<double>();
} else if (opVal.IsHolding<float>()) {
doubleVal = opVal.UncheckedGet<float>();
} else if (opVal.IsHolding<GfHalf>()) {
doubleVal = opVal.UncheckedGet<GfHalf>();
} else {
isScalarVal = false;
}
if (isScalarVal) {
if (isInverseOp)
doubleVal = -doubleVal;
if (opType == TypeRotateX) {
return GfMatrix4d(1.).SetRotate(GfRotation(GfVec3d::XAxis(), doubleVal));
} else if (opType == TypeRotateY) {
return GfMatrix4d(1.).SetRotate(GfRotation(GfVec3d::YAxis(), doubleVal));
} else if (opType == TypeRotateZ) {
return GfMatrix4d(1.).SetRotate(GfRotation(GfVec3d::ZAxis(), doubleVal));
}
}
GfVec3d vec3dVal = GfVec3d(0.);
bool isVecVal = true;
if (opVal.IsHolding<GfVec3f>()) {
vec3dVal = opVal.UncheckedGet<GfVec3f>();
} else if (opVal.IsHolding<GfVec3d>()) {
vec3dVal = opVal.UncheckedGet<GfVec3d>();
} else if (opVal.IsHolding<GfVec3h>()) {
vec3dVal = opVal.UncheckedGet<GfVec3h>();
} else {
isVecVal = false;
}
if (isVecVal) {
switch(opType) {
case TypeTranslate:
if (isInverseOp)
vec3dVal = -vec3dVal;
return GfMatrix4d(1.).SetTranslate(vec3dVal);
case TypeScale:
if (isInverseOp) {
vec3dVal = GfVec3d(1/vec3dVal[0],
1/vec3dVal[1],
1/vec3dVal[2]);
}
return GfMatrix4d(1.).SetScale(vec3dVal);
default: {
if (isInverseOp)
vec3dVal = -vec3dVal;
// Must be one of the 3-axis rotates.
GfMatrix3d xRot(GfRotation(GfVec3d::XAxis(), vec3dVal[0]));
GfMatrix3d yRot(GfRotation(GfVec3d::YAxis(), vec3dVal[1]));
GfMatrix3d zRot(GfRotation(GfVec3d::ZAxis(), vec3dVal[2]));
GfMatrix3d rotationMat(1.);
switch (opType) {
case TypeRotateXYZ:
// Inv(ABC) = Inv(C) * Inv(B) * Inv(A)
rotationMat = !isInverseOp ? (xRot * yRot * zRot)
: (zRot * yRot * xRot);
break;
case TypeRotateXZY:
rotationMat = !isInverseOp ? (xRot * zRot * yRot)
: (yRot * zRot * xRot);
break;
case TypeRotateYXZ:
rotationMat = !isInverseOp ? (yRot * xRot * zRot)
: (zRot * xRot * yRot);
break;
case TypeRotateYZX:
rotationMat = !isInverseOp ? (yRot * zRot * xRot)
: (xRot * zRot * yRot);
break;
case TypeRotateZXY:
rotationMat = !isInverseOp ? (zRot * xRot * yRot)
: (yRot * xRot * zRot);
break;
case TypeRotateZYX:
rotationMat = !isInverseOp ? (zRot * yRot * xRot)
: (xRot * yRot * zRot);
break;
default:
TF_CODING_ERROR("Invalid combination of opType (%s) "
"and opVal (%s). Returning identity matrix.",
TfEnum::GetName(opType).c_str(),
TfStringify(opVal).c_str());
return GfMatrix4d(1.);
}
return GfMatrix4d(1.).SetRotate(rotationMat);
}
}
}
if (opType == TypeOrient) {
GfQuatd quatVal(0);
if (opVal.IsHolding<GfQuatd>())
quatVal = opVal.UncheckedGet<GfQuatd>();
else if (opVal.IsHolding<GfQuatf>()) {
const GfQuatf &quatf = opVal.UncheckedGet<GfQuatf>();
quatVal = GfQuatd(quatf.GetReal(), quatf.GetImaginary());
} else if (opVal.IsHolding<GfQuath>()) {
const GfQuath &quath = opVal.UncheckedGet<GfQuath>();
quatVal = GfQuatd(quath.GetReal(), quath.GetImaginary());
}
GfRotation quatRotation(quatVal);
if (isInverseOp)
quatRotation = quatRotation.GetInverse();
return GfMatrix4d(quatRotation, GfVec3d(0.));
}
TF_CODING_ERROR("Invalid combination of opType (%s) and opVal (%s). "
"Returning identity matrix.", TfEnum::GetName(opType).c_str(),
TfStringify(opVal).c_str());
return GfMatrix4d(1.);
}
void TurretShape::_updateNodes(const Point3F& rot)
{
EulerF xRot(rot.x, 0.0f, 0.0f);
EulerF zRot(0.0f, 0.0f, rot.z);
// Set heading
S32 node = mDataBlock->headingNode;
if (node != -1)
{
MatrixF* mat = &mShapeInstance->mNodeTransforms[node];
Point3F defaultPos = mShapeInstance->getShape()->defaultTranslations[node];
Quat16 defaultRot = mShapeInstance->getShape()->defaultRotations[node];
QuatF qrot(zRot);
qrot *= defaultRot.getQuatF();
qrot.setMatrix( mat );
mat->setColumn(3, defaultPos);
}
// Set pitch
node = mDataBlock->pitchNode;
if (node != -1)
{
MatrixF* mat = &mShapeInstance->mNodeTransforms[node];
Point3F defaultPos = mShapeInstance->getShape()->defaultTranslations[node];
Quat16 defaultRot = mShapeInstance->getShape()->defaultRotations[node];
QuatF qrot(xRot);
qrot *= defaultRot.getQuatF();
qrot.setMatrix( mat );
mat->setColumn(3, defaultPos);
}
// Now the mirror direction nodes, if any
for (U32 i=0; i<TurretShapeData::NumMirrorDirectionNodes; ++i)
{
node = mDataBlock->pitchNodes[i];
if (node != -1)
{
MatrixF* mat = &mShapeInstance->mNodeTransforms[node];
Point3F defaultPos = mShapeInstance->getShape()->defaultTranslations[node];
Quat16 defaultRot = mShapeInstance->getShape()->defaultRotations[node];
QuatF qrot(xRot);
qrot *= defaultRot.getQuatF();
qrot.setMatrix( mat );
mat->setColumn(3, defaultPos);
}
node = mDataBlock->headingNodes[i];
if (node != -1)
{
MatrixF* mat = &mShapeInstance->mNodeTransforms[node];
Point3F defaultPos = mShapeInstance->getShape()->defaultTranslations[node];
Quat16 defaultRot = mShapeInstance->getShape()->defaultRotations[node];
QuatF qrot(zRot);
qrot *= defaultRot.getQuatF();
qrot.setMatrix( mat );
mat->setColumn(3, defaultPos);
}
}
mShapeInstance->setDirty(TSShapeInstance::TransformDirty);
}
