void ComputeAABBOnBoneVisitor::computeBoundingBoxOnBones() {
//Perform Updates
updateBones();
updateRigGeometries();
//We have our T pose, we can compute an AABB for each bone
for (BoneList::iterator bone = _bones.begin(); bone != _bones.end(); ++ bone) {
osg::BoundingBox bb;
//For each geometry
for (RigGeometryList::iterator iterator = _rigGeometries.begin(); iterator != _rigGeometries.end(); ++ iterator) {
osgAnimation::RigGeometry* rigGeometry = *iterator;
if(!rigGeometry) continue;
osg::Matrix mtxLocalToSkl = rigGeometry->getWorldMatrices(_root).at(0);
//For each Vertex influence
osgAnimation::VertexInfluenceMap * infMap = rigGeometry->getInfluenceMap();
osgAnimation::VertexInfluenceMap::iterator itMap = infMap->find((*bone)->getName());
if(itMap == infMap->end()) continue;
osg::Vec3Array *vertices = dynamic_cast<osg::Vec3Array*>(rigGeometry->getVertexArray());
if(!vertices) continue;
osgAnimation::VertexInfluence vxtInf = (*itMap).second;
//Expand the boundingBox with each vertex
for(unsigned int j = 0; j < vxtInf.size(); j++) {
if(vxtInf.at(j).second < 10e-2) continue; //Skip vertex if low weight
osg::Vec3 vx = vertices->at(vxtInf.at(j).first);
vx = vx * mtxLocalToSkl;
bb.expandBy(vx);
}
// Compare initial and actual boundingBox if (no change) => no box on bone
if(bb == osg::BoundingBox() || (bb._min.x() == bb._max.x() && bb._min.y() == bb._max.y() && bb._min.z() == bb._max.z())) {
continue;
}
osg::Matrix worldToBone = osg::Matrix::inverse((*bone)->getWorldMatrices(_root).at(0));
if(_createGeometry) {
osg::Geode *g = new osg::Geode;
g->setName("AABB_for_bone_" + (*bone)->getName());
g->addDrawable(createBox(bb, worldToBone));
(*bone)->addChild(g);
}
serializeBoundingBox(bb, worldToBone, *(*bone));
}
}
//Clear geometries
for (RigGeometryList::iterator iterator = _rigGeometries.begin(); iterator != _rigGeometries.end(); ++ iterator) {
osgAnimation::RigGeometry* rigGeometry = *iterator;
if(rigGeometry) {
rigGeometry->copyFrom(*rigGeometry->getSourceGeometry());
rigGeometry->setRigTransformImplementation(0);
}
}
}
void UserPointCloud::initBones() {
// First retrieve the points of the user
PointCloud::update();
ofVec3f zAxis = ofVec3f(0.0f, 0.0f, 1.0f);
ofQuaternion qDefault = ofQuaternion( 0 , zAxis);
ofQuaternion qLeft = ofQuaternion(-90.0 , zAxis);
ofQuaternion qRight = ofQuaternion( 90.0 , zAxis);
ofQuaternion qNeck = ofQuaternion( 180.0, zAxis);
bones[TORSO] .init( XN_SKEL_TORSO, qDefault);
bones[WAIST] .init( XN_SKEL_WAIST, qDefault);
bones[LEFT_SHOULDER] .init( XN_SKEL_LEFT_SHOULDER, qLeft);
bones[LEFT_ELBOW] .init( XN_SKEL_LEFT_ELBOW, qLeft);
bones[RIGHT_SHOULDER].init( XN_SKEL_RIGHT_SHOULDER, qRight);
bones[RIGHT_ELBOW] .init( XN_SKEL_RIGHT_ELBOW, qRight);
bones[LEFT_HIP] .init( XN_SKEL_LEFT_HIP, qDefault);
bones[RIGHT_HIP] .init( XN_SKEL_RIGHT_HIP, qDefault);
bones[LEFT_KNEE] .init( XN_SKEL_LEFT_KNEE, qDefault);
bones[RIGHT_KNEE] .init( XN_SKEL_RIGHT_KNEE, qDefault);
bones[NECK] .init( XN_SKEL_NECK, qNeck);
// Update the bones
updateBones();
for(int i=0; i<kNumTestNodes; i++) bones[i].updateCalibPose();
setBonesLengths();
initVerticesWeights();
bBonesInit = true;
}
void IKTree::reset(int frame)
{
MT_Quaternion q;
float rad;
BVHNode *n;
for (int i=0; i<numBones; i++) {
bone[i].pos = origin;
bone[i].lRot = identity;
bone[i].gRot = identity;
n = bone[i].node;
for (int k=0; k<3; k++) { // rotate each axis in order
rad = n->frame[frame][k] * M_PI / 180;
q = identity;
switch (n->channelType[k]) {
case BVH_XROT: q.setRotation(xAxis, rad); break;
case BVH_YROT: q.setRotation(yAxis, rad); break;
case BVH_ZROT: q.setRotation(zAxis, rad); break;
case BVH_XPOS: bone[i].pos[0] = n->frame[frame][k]; break;
case BVH_YPOS: bone[i].pos[1] = n->frame[frame][k]; break;
case BVH_ZPOS: bone[i].pos[2] = n->frame[frame][k]; break;
}
bone[i].lRot = q * bone[i].lRot;
}
}
updateBones(0);
}
void UserPointCloud::update() {
if (user->getTrackedUsers().size()>0) {
if(!bBonesInit) initBones();
else updateBones();
}
}
void IKTree::solve(int frame)
{
double x, y, z;
reset(frame);
IKEffectorList effList;
for (int i=0; i<20; i++) {
effList.num = 0;
solveJoint(frame, 0, effList);
}
for (int i=0; i<numBones-1; i++) {
BVHNode *n = bone[i].node;
toEuler(bone[i].lRot, n->channelOrder, x, y, z);
for (int j=0; j<3; j++) { // rotate each axis in order
switch (n->channelType[j]) {
case BVH_XROT: n->ikRot[j] = x - n->frame[frame][j]; break;
case BVH_YROT: n->ikRot[j] = y - n->frame[frame][j]; break;
case BVH_ZROT: n->ikRot[j] = z - n->frame[frame][j]; break;
}
}
}
display = 1;
updateBones(0);
display = 0;
}
void GfxBody::setAllBonesManuallyControlled (bool v)
{
if (dead) THROW_DEAD(className);
for (unsigned i=0 ; i<manualBones.size() ; ++i) {
manualBones[i] = v;
}
updateBones();
}
//------------------------------------------- update.
void ofxAssimpModelLoader::update() {
updateAnimations();
updateMeshes(scene->mRootNode, ofMatrix4x4());
if(hasAnimations() == false) {
return;
}
updateBones();
updateGLResources();
}
void IKTree::updateBones(int i)
{
for (int j=0; j<bone[i].numChildren; j++) {
int k = bone[i].child[j];
bone[k].gRot = bone[i].gRot * bone[i].lRot;
MT_Transform rot(MT_Point3(0,0,0), bone[k].gRot);
bone[k].pos = bone[i].pos + rot * bone[k].offset;
updateBones(k);
}
}
void IKTree::reset(int frame)
{
MT_Quaternion q;
BVHNode *n;
for (int i=0; i<numBones; i++) {
bone[i].pos = origin;
bone[i].lRot = identity;
bone[i].gRot = identity;
n = bone[i].node;
Rotation rot=n->frameData(frame).rotation();
Position pos=n->frameData(frame).position();
for (int k=0; k<n->numChannels; k++) { // rotate each axis in order
q = identity;
switch (n->channelType[k]) {
case BVH_XROT:
q.setRotation(xAxis, rot.x * M_PI / 180);
break;
case BVH_YROT:
q.setRotation(yAxis, rot.y * M_PI / 180);
break;
case BVH_ZROT:
q.setRotation(zAxis, rot.z * M_PI / 180);
break;
case BVH_XPOS: bone[i].pos[0] = pos.x; break;
case BVH_YPOS: bone[i].pos[1] = pos.y; break;
case BVH_ZPOS: bone[i].pos[2] = pos.z; break;
}
bone[i].lRot = q * bone[i].lRot;
}
/*
for (int k=0; k<3; k++) { // rotate each axis in order
rad = n->frame[frame][k] * M_PI / 180;
q = identity;
switch (n->channelType[k]) {
case BVH_XROT: q.setRotation(xAxis, rad); break;
case BVH_YROT: q.setRotation(yAxis, rad); break;
case BVH_ZROT: q.setRotation(zAxis, rad); break;
case BVH_XPOS: bone[i].pos[0] = n->frame[frame][k]; break;
case BVH_YPOS: bone[i].pos[1] = n->frame[frame][k]; break;
case BVH_ZPOS: bone[i].pos[2] = n->frame[frame][k]; break;
}
bone[i].lRot = q * bone[i].lRot;
}
*/
}
updateBones(0);
}
void GfxBody::reinitialise (void)
{
APP_ASSERT(mesh->isLoaded());
destroyGraphics();
for (unsigned short i = 0; i < mesh->getNumSubMeshes(); ++i) {
Ogre::SubMesh *sm = mesh->getSubMesh(i);
Sub* sub = new Sub(this, sm);
subList.push_back(sub);
GFX_MAT_SYNC;
std::string matname = apply_map(initialMaterialMap, sm->getMaterialName());
if (!gfx_material_has(matname)) {
CERR << "Mesh \"/"<<mesh->getName()<<"\" references non-existing material "
<< "\""<<matname<<"\""<<std::endl;
matname = "/system/FallbackMaterial";
}
sub->material = gfx_material_get(matname);
}
if (!mesh->getSkeleton().isNull()) {
skeleton = OGRE_NEW Ogre::SkeletonInstance(mesh->getSkeleton());
skeleton->load();
numBoneMatrixes = skeleton->getNumBones();
boneMatrixes = static_cast<Ogre::Matrix4*>(OGRE_MALLOC_SIMD(sizeof(Ogre::Matrix4) * numBoneMatrixes, Ogre::MEMCATEGORY_ANIMATION));
boneWorldMatrixes = static_cast<Ogre::Matrix4*>(OGRE_MALLOC_SIMD(sizeof(Ogre::Matrix4) * numBoneMatrixes, Ogre::MEMCATEGORY_ANIMATION));
mesh->_initAnimationState(&animationState);
} else {
skeleton = NULL;
numBoneMatrixes = 0;
boneMatrixes = NULL;
boneWorldMatrixes = NULL;
}
updateBones();
}
void GfxBody::setBoneManuallyControlled (unsigned n, bool v)
{
checkBone(n);
manualBones[n] = v;
updateBones();
}
void updateEditor(SDL_Event event, EditorData* data)
{
GUI_sendEventToGUI(data->gui, &event);
const unsigned char* keyboardState = SDL_GetKeyboardState(NULL);
int2 mousePixel;
SDL_GetMouseState(&mousePixel.x, &mousePixel.y);
float2 mousePos = pixelToPoint(&data->view, mousePixel);
updateBones(data);
switch (event.type)
{
case SDL_KEYDOWN:
{
switch (event.key.keysym.scancode)
{
case SDL_SCANCODE_S:
if (event.key.keysym.mod == KMOD_LCTRL)
{
SDL_Event lastEvent = data->lowEventStack.Top();
if (lastEvent.key.keysym.scancode == SDL_SCANCODE_S && lastEvent.key.keysym.mod == KMOD_LCTRL)
{
// save skeleton
GUI_WidgetID widget = GUI_addContainer(data->gui, GUI_MAIN_WIDGET, { 400 - 100, 300 - 25, 200, 50}, GUI_Container::Orientation::HORIZONTAL);
auto callbackSave = [](GUI* gui, GUI_WidgetID widgetID, void* editorData)
{
EditorData* data = (EditorData*)editorData;
GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID);
const char* filename = GUI_getTextfield(gui, GUI_getChildWidget(gui, widget, 0)).text;
saveSkeleton(filename, &data->skeleton[0], &data->names[0], data->skeleton.size());
GUI_removeWidget(gui, widget);
};
auto callbackCancel = [](GUI* gui, GUI_WidgetID widgetID, void*)
{
GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID);
GUI_removeWidget(gui, widget);
};
GUI_addTextfield(data->gui, widget, { 0, 0, -1, -1 }, "filename");
GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Save", data, callbackSave);
GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Cancel", 0, callbackCancel);
break;
}
else if (lastEvent.key.keysym.scancode == SDL_SCANCODE_O && lastEvent.key.keysym.mod == KMOD_LCTRL)
{
// load skeleton
GUI_WidgetID widget = GUI_addContainer(data->gui, GUI_MAIN_WIDGET, { 400 - 100, 300 - 15, 200, 30 }, GUI_Container::Orientation::HORIZONTAL);
auto callbackLoad = [](GUI* gui, GUI_WidgetID widgetID, void* editorData)
{
EditorData* data = (EditorData*)editorData;
GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID);
const char* filename = GUI_getTextfield(gui, GUI_getChildWidget(gui, widget, 0)).text;
strcpy(data->skeletonName, filename);
const SkeletonSave* save = loadSkeleton(filename);
if (save != 0)
{
data->bones.clear();
data->skeleton.clear();
data->constraints.clear();
data->names.clear();
const char* name = save->names;
for (int i = 0; i < save->numBones; ++i)
{
Bone newBone;
newBone.jointAngle = { 1, 0 };
newBone.jointPos = save->jointPos[i];
newBone.parentJoint = save->parentJoints[i];
newBone.name = data->names.push({ { 0 } });
memcpy(data->names[newBone.name].string, name, save->nameLen[i] * sizeof(char));
name += save->nameLen[i];
Constraint constraint;
constraint.minAngle = -PI;
constraint.maxAngle = 3 * PI;
data->skeleton.push(newBone);
data->constraints.push(constraint);
}
updateBones(data);
}
GUI_removeWidget(gui, widget);
};
auto callbackCancel = [](GUI* gui, GUI_WidgetID widgetID, void*)
{
GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID);
GUI_removeWidget(gui, widget);
};
GUI_addTextfield(data->gui, widget, { 0, 0, -1, -1 }, "filename");
GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Load", data, callbackLoad);
GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Cancel", 0, callbackCancel);
break;
}
data->lowEventStack.Push(event);
}
break;
case SDL_SCANCODE_K:
if (event.key.keysym.mod == KMOD_LCTRL)
{
SDL_Event lastEvent = data->lowEventStack.Top();
if (lastEvent.key.keysym.scancode == SDL_SCANCODE_S && lastEvent.key.keysym.mod == KMOD_LCTRL)
{
// save keyFrame
GUI_WidgetID widget = GUI_addContainer(data->gui, GUI_MAIN_WIDGET, { 400 - 100, 300 - 15, 200, 30 }, GUI_Container::Orientation::HORIZONTAL);
auto callbackSave = [](GUI* gui, GUI_WidgetID widgetID, void* editorData)
{
EditorData* data = (EditorData*)editorData;
GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID);
const char* filename = GUI_getTextfield(gui, GUI_getChildWidget(gui, widget, 0)).text;
saveKeyFrame(filename, &data->skeleton[0], &data->names[0], data->skeleton.size());
GUI_removeWidget(gui, widget);
};
auto callbackCancel = [](GUI* gui, GUI_WidgetID widgetID, void*)
{
GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID);
GUI_removeWidget(gui, widget);
};
GUI_addTextfield(data->gui, widget, { 0, 0, -1, -1 }, "filename");
GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Save", data, callbackSave);
GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Cancel", 0, callbackCancel);
break;
}
else if (lastEvent.key.keysym.scancode == SDL_SCANCODE_O && lastEvent.key.keysym.mod == KMOD_LCTRL)
{
// load keyframe
GUI_WidgetID widget = GUI_addContainer(data->gui, GUI_MAIN_WIDGET, { 400 - 100, 300 - 15, 200, 30 }, GUI_Container::Orientation::HORIZONTAL);
auto callbackLoad = [](GUI* gui, GUI_WidgetID widgetID, void* editorData)
{
EditorData* data = (EditorData*)editorData;
GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID);
const char* filename = GUI_getTextfield(gui, GUI_getChildWidget(gui, widget, 0)).text;
strcpy(data->frameName, filename);
const KeyFrameSave* save = loadKeyFrame(filename);
if (save != 0)
{
const char* name = save->names;
for (int i = 0; i < save->numBones; ++i)
{
int nameID = -1;
for (int j = 0; j < data->names.size(); ++j)
{
if (strncmp(name, data->names[j].string, save->nameLen[i]) == 0)
{
nameID = j;
break;
}
}
if (nameID != -1)
{
if (data->skeleton[nameID].name == nameID)
{
data->skeleton[nameID].jointAngle = save->jointAngles[i];
}
else
{
for (int j = 0; j < data->skeleton.size(); j++)
{
if (data->skeleton[j].name = nameID)
data->skeleton[j].jointAngle = save->jointAngles[i];
}
}
}
name += save->nameLen[i];
}
updateBones(data);
}
GUI_removeWidget(gui, widget);
};
auto callbackCancel = [](GUI* gui, GUI_WidgetID widgetID, void*)
{
GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID);
GUI_removeWidget(gui, widget);
};
GUI_addTextfield(data->gui, widget, { 0, 0, -1, -1 }, "filename");
GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Load", data, callbackLoad);
GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Cancel", 0, callbackCancel);
break;
}
data->lowEventStack.Push(event);
}
break;
default:
data->lowEventStack.Push(event);
break;
}
break;
}
case SDL_MOUSEBUTTONDOWN:
{
data->newBone = -1;
if (keyboardState[SDL_SCANCODE_A])
{
for (int i = 0; i < data->bones.size(); ++i)
{
if (pointInRect(mousePos, data->bones[i].rect))
{
int name = data->names.push({ "test" });
Bone newBone;
newBone.jointAngle = { 1, 0 };
newBone.jointPos = { 0, 0 };
newBone.parentJoint = i;
newBone.name = name;
Constraint constraint;
constraint.minAngle = -PI;
constraint.maxAngle = 3 * PI;
int newBoneID = data->skeleton.push(newBone);
data->constraints.push(constraint);
int* ids = new int[data->skeleton.size()];
sortSkeleton(&data->skeleton[0], data->skeleton.size(), ids);
Constraint* tempConstraints = new Constraint[data->constraints.size()];
memcpy(tempConstraints, &data->constraints[0], sizeof(Constraint) * data->constraints.size());
for (int i = 0; i < data->constraints.size(); ++i)
{
data->constraints[i] = tempConstraints[ids[i]];
}
delete[] tempConstraints;
//find bone again
data->newBone = ids[newBoneID];
// rebuild bone selection
updateBones(data);
delete[]ids;
break;
}
}
}
else if (keyboardState[SDL_SCANCODE_N])
{
for (int i = 0; i < data->bones.size(); ++i)
{
if (pointInRect(mousePos, data->bones[i].rect))
{
int2 pos = pointToPixel(&data->view, float2{ data->bones[i].rect.x + 10, data->bones[i].rect.y });
GUI_WidgetID widget = GUI_addContainer(data->gui, GUI_MAIN_WIDGET, intRect{ pos.x, pos.y, 100, 30 }, GUI_Container::HORIZONTAL);
struct Args
{
EditorData* data;
int boneId;
}*args = new Args{ data, i };
auto callbackOk = [](GUI* gui, GUI_WidgetID widgetID, void* args)
{
EditorData* data = ((Args*)args)->data;
int boneID = ((Args*)args)->boneId;
GUI_WidgetID parentID = GUI_getParentWidget(gui, widgetID);
const char* newName = GUI_getTextfield(gui, GUI_getChildWidget(gui, parentID, 0)).text;
strcpy(data->names[data->skeleton[boneID].name].string, newName);
GUI_removeWidget(gui, parentID);
delete args;
};
GUI_addTextfield(data->gui, widget, intRect{ 0, 0, -1, -1 }, data->names[data->skeleton[i].name].string);
GUI_addButton(data->gui, widget, intRect{ 0, 0, 30, -1 }, "OK", args, callbackOk);
break;
}
}
}
else
{
for (int i = 0; i < data->bones.size(); i++)
{
if (pointInRect(mousePos, data->bones[i].rect))
{
data->selectedBones.push(i);
data->grabbedBone = i;
}
}
if (data->grabbedBone == -1 && !keyboardState[SDL_SCANCODE_LCTRL])
data->selectedBones.clear();
}
break;
}
case SDL_MOUSEBUTTONUP:
{
data->grabbedBone = -1;
break;
}
case SDL_MOUSEMOTION:
{
if (SDL_GetMouseState(0, 0) == SDL_BUTTON_MIDDLE)
{
data->view.position.x -= (event.motion.xrel / data->view.scale.x);
data->view.position.y -= (event.motion.yrel / data->view.scale.y);
}
else
{
float2 motion = rotate({ (float)event.motion.xrel, (float)event.motion.yrel }, data->view.angle) * float2{ 1 / data->view.scale.x, 1 / data->view.scale.y };
if (data->newBone != -1)
{
float2 angle = getBoneWorldTransform(&data->skeleton[0], data->skeleton.size(), data->newBone).angle;
float2 rel = rotate(float2{ motion.x, motion.y }, negateRotation(angle));
data->skeleton[data->newBone].jointPos += rel;
}
if (data->grabbedBone != -1)
{
if (keyboardState[SDL_SCANCODE_LCTRL])
{
float2 angle = getBoneWorldTransform(&data->skeleton[0], data->skeleton.size(), data->skeleton[data->grabbedBone].parentJoint).angle;
float2 rel = rotate(float2{ motion.x, motion.y }, negateRotation(angle));
data->skeleton[data->grabbedBone].jointPos += rel;
}
else
{
animateCCDIKSelection(&data->skeleton[0], &data->constraints[0], data->skeleton.size(),
&data->selectedBones[0], data->selectedBones.size(), data->grabbedBone, mousePos - float2{ 400, 300 });
}
}
}
break;
}
case SDL_MOUSEWHEEL:
{
data->view.scale.x += event.wheel.y*0.1f;
data->view.scale.y += event.wheel.y*0.1f;
break;
}
}
}
void IKTree::solveJoint(int frame, int i, IKEffectorList &effList)
{
double x, y, z;
double ang = 0;
MT_Quaternion q;
MT_Quaternion totalPosRot = MT_Quaternion(0,0,0,0);
MT_Quaternion totalDirRot = MT_Quaternion(0,0,0,0);
MT_Vector3 axis(0,0,0);
BVHNode *n;
int numPosRot = 0, numDirRot = 0;
if (bone[i].numChildren == 0) { // reached end site
if (bone[i].node->ikOn) {
effList.index[effList.num++] = i;
}
return;
}
for (int j=0; j<bone[i].numChildren; j++) {
IKEffectorList el;
el.num = 0;
solveJoint(frame, bone[i].child[j], el);
for (int k=0; k<el.num; k++) {
effList.index[effList.num++] = el.index[k];
}
}
updateBones(i);
for (int j=0; j<effList.num; j++) {
int effIndex = effList.index[j];
n = bone[effIndex].node;
MT_Vector3 effGoalPos(n->ikGoalPos[0],
n->ikGoalPos[1],
n->ikGoalPos[2]);
const MT_Vector3 pC =
(bone[effIndex].pos - bone[i].pos).safe_normalized();
const MT_Vector3 pD =
(effGoalPos - bone[i].pos).safe_normalized();
MT_Vector3 rotAxis = pC.cross(pD);
if (rotAxis.length2() > MT_EPSILON) {
totalPosRot += MT_Quaternion(rotAxis, bone[i].weight * acos(pC.dot(pD)));
numPosRot++;
}
const MT_Vector3 uC =
(bone[effIndex].pos - bone[effIndex-1].pos).safe_normalized();
const MT_Vector3 uD =
(MT_Vector3(n->ikGoalDir[0], n->ikGoalDir[1], n->ikGoalDir[2])).safe_normalized();
rotAxis = uC.cross(uD);
if (rotAxis.length2() > MT_EPSILON) {
double weight = 0.0;
if (i == effIndex-1) weight = 0.5;
totalDirRot += MT_Quaternion(rotAxis, weight * acos(uC.dot(uD)));
numDirRot++;
}
}
if ((numPosRot + numDirRot) > MT_EPSILON) {
n = bone[i].node;
n->ikOn = true;
// average the quaternions from all effectors
if (numPosRot)
totalPosRot /= numPosRot;
else
totalPosRot = identity;
if (numDirRot)
totalDirRot /= numDirRot;
else
totalDirRot = identity;
MT_Quaternion targetRot = 0.9 * totalPosRot + 0.1 * totalDirRot;
targetRot = targetRot * bone[i].lRot;
toEuler(targetRot, n->channelOrder, x, y, z);
if (jointLimits) {
bone[i].lRot = identity;
for (int k=0; k<n->numChannels; k++) { // clamp each axis in order
switch (n->channelType[k]) {
case BVH_XROT: ang = x; axis = xAxis; break;
case BVH_YROT: ang = y; axis = yAxis; break;
case BVH_ZROT: ang = z; axis = zAxis; break;
default: break;
}
// null axis leads to crash in q.setRotation(), so check first
if(axis.length())
{
if (ang < n->channelMin[k]) ang = n->channelMin[k];
else if (ang > n->channelMax[k]) ang = n->channelMax[k];
q.setRotation(axis, ang * M_PI / 180);
bone[i].lRot = q * bone[i].lRot;
}
}
}
else
bone[i].lRot = targetRot;
}
}
int main( int argc, char **argv )
{
// use an ArgumentParser object to manage the program arguments.
osg::ArgumentParser arguments(&argc,argv);
// set up the usage document, in case we need to print out how to use this program.
arguments.getApplicationUsage()->setApplicationName(arguments.getApplicationName());
arguments.getApplicationUsage()->setDescription(arguments.getApplicationName()+" is the standard OpenSceneGraph example which loads and visualises 3d models.");
arguments.getApplicationUsage()->setCommandLineUsage(arguments.getApplicationName()+" [options] filename ...");
arguments.getApplicationUsage()->addCommandLineOption("--image <filename>","Load an image and render it on a quad");
arguments.getApplicationUsage()->addCommandLineOption("--dem <filename>","Load an image/DEM and render it on a HeightField");
arguments.getApplicationUsage()->addCommandLineOption("-h or --help","Display command line paramters");
arguments.getApplicationUsage()->addCommandLineOption("--help-env","Display environmental variables available");
arguments.getApplicationUsage()->addCommandLineOption("--help-keys","Display keyboard & mouse bindings available");
arguments.getApplicationUsage()->addCommandLineOption("--help-all","Display all command line, env vars and keyboard & mouse bindigs.");
int numLimbs = 3;
while (arguments.read("--limbs", numLimbs)) {}
// construct the viewer.
osgProducer::Viewer viewer(arguments);
// set up the value with sensible default event handlers.
viewer.setUpViewer(osgProducer::Viewer::STANDARD_SETTINGS);
#if 0
unsigned int pos = viewer.addCameraManipulator(new GliderManipulator());
viewer.selectCameraManipulator(pos);
#endif
// get details on keyboard and mouse bindings used by the viewer.
viewer.getUsage(*arguments.getApplicationUsage());
// if user request help write it out to cout.
bool helpAll = arguments.read("--help-all");
unsigned int helpType = ((helpAll || arguments.read("-h") || arguments.read("--help"))? osg::ApplicationUsage::COMMAND_LINE_OPTION : 0 ) |
((helpAll || arguments.read("--help-env"))? osg::ApplicationUsage::ENVIRONMENTAL_VARIABLE : 0 ) |
((helpAll || arguments.read("--help-keys"))? osg::ApplicationUsage::KEYBOARD_MOUSE_BINDING : 0 );
if (helpType)
{
arguments.getApplicationUsage()->write(std::cout, helpType);
return 1;
}
// report any errors if they have occured when parsing the program aguments.
if (arguments.errors())
{
arguments.writeErrorMessages(std::cout);
return 1;
}
osg::Timer_t start_tick = osg::Timer::instance()->tick();
// any option left unread are converted into errors to write out later.
arguments.reportRemainingOptionsAsUnrecognized();
// report any errors if they have occured when parsing the program aguments.
if (arguments.errors())
{
arguments.writeErrorMessages(std::cout);
}
osg::Timer_t end_tick = osg::Timer::instance()->tick();
std::cout << "Time to load = "<<osg::Timer::instance()->delta_s(start_tick,end_tick)<<std::endl;
// optimize the scene graph, remove rendundent nodes and state etc.
osgUtil::Optimizer optimizer;
/// set background color to black
viewer.setClearColor(osg::Vec4(0.95,0.95,0.95,1.0) );
scene = new osg::Group;
osg::StateSet* rootStateSet = new osg::StateSet;
scene->setStateSet(rootStateSet);
srand((unsigned)time(0));
// pass the loaded scene graph to the viewer.
viewer.setSceneData(scene);
bone* rootBone = makeRandomBone(numLimbs,osg::Vec3(0.0,10.0,0.0),15.0f);
std::cout << "allBones.size() " << allBones.size() << std::endl;
/// duplicate the bones thing all over
for (unsigned i = 0; i < 25; i++) {
osg::PositionAttitudeTransform* randPlace = new osg::PositionAttitudeTransform;
/// just having random positions looks pretty cool
randPlace->setPosition(osg::Vec3(randomf(),randomf(),randomf() )*200.0f );
osg::Quat quat = osg::Quat(
randomf(0,M_PI), osg::Vec3(1,0,0),
randomf(0,M_PI), osg::Vec3(0,1,0),
randomf(0,M_PI), osg::Vec3(0,0,1)
);
randPlace->setAttitude(quat);
randPlace->addChild(rootBone->root);
scene->addChild(randPlace);
}
#ifdef VECS2
for (unsigned i = 0; i < allBones.size(); i++) {
scene->addChild(allBones[i]->objposScene);
}
#endif
if (1) {
osg::StateSet* ss = scene->getOrCreateStateSet();
osg::Program* BlockyProgram = new osg::Program;
BlockyProgram->setName( "blocky" );
osg::Shader* BlockyVertObj = new osg::Shader( osg::Shader::VERTEX );
osg::Shader* BlockyFragObj = new osg::Shader( osg::Shader::FRAGMENT );
BlockyProgram->addShader( BlockyFragObj );
BlockyProgram->addShader( BlockyVertObj );
ss->setAttributeAndModes(BlockyProgram, osg::StateAttribute::ON);
BlockyVertObj->loadShaderSourceFromFile("shaders/blocky.vert");
BlockyFragObj->loadShaderSourceFromFile("shaders/blocky.frag");
//for unsigned i = 0
//viewer.getSceneHandlerList()[0]->getSceneView()->setActiveUniforms(osgUtil::SceneView::VIEW_MATRIX_INVERSE_UNIFORM);
}
// create the windows and run the threads.
viewer.realize();
int i = 0;
float preIncr = 0.0;
while( !viewer.done())
{
// wait for all cull and draw threads to complete.
viewer.sync();
// update the scene by traversing it with the the update visitor which will
// call all node update callbacks and animations.
viewer.update();
// fire off the cull and draw traversals of the scene.
viewer.frame();
usleep(500);
preIncr += 0.005;
updateBones(preIncr);
if (1) {
std::stringstream imageName;
imageName << "images/image_" << i << ".jpg";
screenCapture(&viewer, imageName.str());
i++;
}
}
// wait for all cull and draw threads to complete before exit.
viewer.sync();
// run a clean up frame to delete all OpenGL objects.
viewer.cleanup_frame();
// wait for all the clean up frame to complete.
viewer.sync();
return 0;
}
