/**
@brief Wait until the kinect handler updated
*/
void Kinect::WaitAndUpdate(UINT flag)
{
std::vector< HANDLE > handle;
if ( VideoStream().hStream_ != NULL && (flag & UPDATE_FLAG_VIDEO)) {
handle.push_back( VideoStream().event_.get() );
}
if ( DepthStream().hStream_ != NULL && (flag & UPDATE_FLAG_DEPTH)) {
handle.push_back( DepthStream().event_.get() );
}
if( AudioStream().hStream_ != NULL && (flag & UPDATE_FLAG_AUDIO)) {
handle.push_back( AudioStream().event_.get() );
}
if ( Skeleton().IsEnabled() && (flag & UPDATE_FLAG_SKELETON)) {
handle.push_back( Skeleton().event_.get() );
}
if ( handle.size() == 0 ) {
return;
}
DWORD ret = ::WaitForMultipleObjects( handle.size(), &handle[0], TRUE, INFINITE );
if ( ret == WAIT_FAILED ) {
return;
}
}
/**
@brief Wait until all the kinect handler updated
*/
void Kinect::WaitAndUpdateAll()
{
std::vector< HANDLE > handle;
if ( VideoStream().hStream_ != NULL ) {
handle.push_back( VideoStream().event_.get() );
}
if ( DepthStream().hStream_ != NULL ) {
handle.push_back( DepthStream().event_.get() );
}
if( AudioStream().hStream_ != NULL ) {
handle.push_back( AudioStream().event_.get() );
}
if ( Skeleton().IsEnabled() ) {
handle.push_back( Skeleton().event_.get() );
}
if ( handle.size() == 0 ) {
return;
}
DWORD ret = ::WaitForMultipleObjects( handle.size(), &handle[0], TRUE, INFINITE );
if ( ret == WAIT_FAILED ) {
return;
}
}
void SpinningCube::Draw() {
//glMatrixMode(GL_MODELVIEW);
//glLoadMatrixf(WorldMtx);
//glutWireCube(Size);
/*
Joint test = Joint("Test");
test.setPose(Vector3(45, 45, 0));
test.setTranslation(Vector3(0, 0, 0));
test.setScale(Vector3(1, 1, 1), Vector3(0, 0, 0));
test.calculate();
Joint test1 = Joint("Test2");
test1.setDOFX(0);
test1.setDOFY(0);
test1.setDOFZ(0);
test1.setTranslation(Vector3(0, 1.5, 0));
test1.setScale(Vector3(1, 1, 1), Vector3(0, 0, 0));
test1.calculate();
test.addChild(test1);
Joint *test2 = test1.getParent();
test.draw(WorldMtx.IDENTITY);*/
Skeleton test = Skeleton();
test.Load("test.skel");
test.calculate(WorldMtx.IDENTITY);
test.draw();
}
Kinect::Kinect()
{
NuiSetDeviceStatusCallback( &KinectSdk::deviceStatus, this );
init();
for ( int32_t i = 0; i < NUI_SKELETON_COUNT; i++ ) {
mSkeletons.push_back( Skeleton() );
}
}
Device::Device()
{
NuiSetDeviceStatusCallback( &MsKinect::deviceStatus, this );
init();
for ( int32_t i = 0; i < NUI_SKELETON_COUNT; ++i ) {
mSkeletons.push_back( Skeleton() );
}
App::get()->getSignalUpdate().connect( bind( &Device::update, this ) );
}
/**
* Get the skeleton data
* Returns the detected skeleton data from the kinect as defined in the Kinect SDK
* @param skeletonIndex Which of (potentially 2) skeletons to return. This is ignored in this implementation and
* only a single skeleton is supported for the FRC release default gesture interpretation.
* @return The current version of the skeleton object.
*/
Skeleton Kinect::GetSkeleton(int skeletonIndex)
{
if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Skeleton index must be 1");
return Skeleton();
}
UpdateData();
return m_skeletons[skeletonIndex-1];
}
void Kinect::enableSkeletons( bool enable )
{
bool toggle = mEnabledSkeletons != enable;
mEnabledSkeletons = enable;
if ( toggle ) {
if ( !mEnabledSkeletons ) {
mFrameRateSkeletons = 0.0f;
}
mSkeletons.clear();
for ( int32_t i = 0; i < NUI_SKELETON_COUNT; i++ ) {
mSkeletons.push_back( Skeleton() );
}
}
}
void skeleton_new_cb( Fl_Widget* w, void* ) {
static int nb_tot = 2;
const char* p = fl_input( _("Number of functions (1->%d) ?"),
IS::translate(nb_tot).c_str(),
Skeleton::NBM - 1 );
if ( p != NULL ) {
nb_tot = atoi(p);
if ( 1 <= nb_tot && nb_tot <= Skeleton::NBM - 1) {
glito->skel = Skeleton( nb_tot );
glito->state = PREVIEW;
glito->needRedraw = true;
} else {
fl_alert( _("The number of functions must be between 1 and %d"),
Skeleton::NBM - 1 );
}
}
}
SkeletonFrame::SkeletonFrame( KinectSensor& kinect, SkeletonStream& skeletonStream, DWORD timeout, NUI_TRANSFORM_SMOOTH_PARAMETERS* smooth )
: kinect_( kinect )
, skeletonStream_( skeletonStream )
{
kinect_->NuiSkeletonGetNextFrame( timeout, &frame_ );
if ( smooth != 0 ) {
kinect_->NuiTransformSmooth( &frame_, smooth );
}
skeletonData_.reserve( NUI_SKELETON_COUNT );
for ( int i = 0; i < NUI_SKELETON_COUNT; ++i ) {
skeletonData_.push_back( Skeleton( frame_.SkeletonData[i] ) );
}
//for ( auto s : frame_.SkeletonData ) {
// skeletonData_.push_back( s );
//}
}
void Skel_OPB_Comp::calc_buf(INT ** output,U_INT1 *** input)
{
if (first_line_in_pack())
{
for (INT d =0; d < dim_in() ; d++)
{
U_INT1 ** l = input[d];
for (int y = y0Buf() ; y < y1Buf() ; y++)
convert
(
_im_init[y-y0Buf()],
l[y]+x0Buf(),
x1Buf()-x0Buf()
);
Skeleton
(
_skel[d],
_im_init,
_sz.x,
_sz.y,
_larg
);
}
}
for (INT d =0; d < dim_in() ; d++)
convert
(
output[d]+x0(),
_skel[d][y_in_pack()-dy0()]-dx0(),
tx()
);
if (_AvecDist)
convert
(
output[1]+x0(),
_im_init[y_in_pack()-dy0()]-dx0(),
tx()
);
}
//-----------------------------------------------------------------------
Resource* SkeletonManager::createImpl(const String& name, ResourceHandle handle,
const String& group, bool isManual, ManualResourceLoader* loader,
const NameValuePairList* createParams)
{
return OGRE_NEW Skeleton(this, name, handle, group, isManual, loader);
}
void StdMeshSkeletonLoader::LoadSkeletonXml(const char* groupname, const char* filename, const char *sourcefile, size_t size)
{
if (sourcefile == NULL)
{
throw Ogre::InsufficientData(FormatString("Failed to load '%s/%s'", groupname, filename).getData());
}
std::shared_ptr<StdMeshLoader::StdMeshXML> skeleton(new StdMeshLoader::StdMeshXML(filename, sourcefile));
TiXmlElement* skeleton_elem = skeleton->RequireFirstChild(NULL, "skeleton");
TiXmlElement* bones_elem = skeleton->RequireFirstChild(skeleton_elem, "bones");
// Read bones. Don't insert into Master bone table yet, as the master bone
// table is sorted hierarchically, and we will read the hierarchy only
// afterwards.
std::vector<StdMeshBone*> bones;
for (TiXmlElement* bone_elem = bones_elem->FirstChildElement("bone"); bone_elem != NULL; bone_elem = bone_elem->NextSiblingElement("bone"))
{
StdMeshBone* bone = new StdMeshBone;
bones.push_back(bone);
bone->ID = skeleton->RequireIntAttribute(bone_elem, "id");
bone->Name = skeleton->RequireStrAttribute(bone_elem, "name");
// TODO: Make sure ID and name are unique
bone->Parent = NULL;
// Index of bone will be set when building Master Bone Table later
TiXmlElement* position_elem = skeleton->RequireFirstChild(bone_elem, "position");
TiXmlElement* rotation_elem = skeleton->RequireFirstChild(bone_elem, "rotation");
TiXmlElement* axis_elem = skeleton->RequireFirstChild(rotation_elem, "axis");
StdMeshVector d, r;
d.x = skeleton->RequireFloatAttribute(position_elem, "x");
d.y = skeleton->RequireFloatAttribute(position_elem, "y");
d.z = skeleton->RequireFloatAttribute(position_elem, "z");
float angle = skeleton->RequireFloatAttribute(rotation_elem, "angle");
r.x = skeleton->RequireFloatAttribute(axis_elem, "x");
r.y = skeleton->RequireFloatAttribute(axis_elem, "y");
r.z = skeleton->RequireFloatAttribute(axis_elem, "z");
bone->Transformation.scale = StdMeshVector::UnitScale();
bone->Transformation.rotate = StdMeshQuaternion::AngleAxis(angle, r);
bone->Transformation.translate = d;
// We need this later for applying animations, and attaching meshes, therefore cache it here
bone->InverseTransformation = StdMeshTransformation::Inverse(bone->Transformation);
}
// Bone hierarchy
TiXmlElement* bonehierarchy_elem = skeleton->RequireFirstChild(skeleton_elem, "bonehierarchy");
for (TiXmlElement* boneparent_elem = bonehierarchy_elem->FirstChildElement("boneparent"); boneparent_elem != NULL; boneparent_elem = boneparent_elem->NextSiblingElement("boneparent"))
{
const char* child_name = skeleton->RequireStrAttribute(boneparent_elem, "bone");
const char* parent_name = skeleton->RequireStrAttribute(boneparent_elem, "parent");
// Lookup the two bones
StdMeshBone* child = NULL;
StdMeshBone* parent = NULL;
for (unsigned int i = 0; i < bones.size() && (!child || !parent); ++i)
{
if (!child && bones[i]->Name == child_name)
child = bones[i];
if (!parent && bones[i]->Name == parent_name)
parent = bones[i];
}
if (!child) skeleton->Error(FormatString("There is no such bone with name '%s'", child_name), boneparent_elem);
if (!parent) skeleton->Error(FormatString("There is no such bone with name '%s'", parent_name), boneparent_elem);
child->Parent = parent;
parent->Children.push_back(child);
}
std::shared_ptr<StdMeshSkeleton> Skeleton(new StdMeshSkeleton);
// Fill master bone table in hierarchical order:
for (unsigned int i = 0; i < bones.size(); ++i)
if (bones[i]->Parent == NULL)
Skeleton->AddMasterBone(bones[i]);
// Load Animations
TiXmlElement* animations_elem = skeleton_elem->FirstChildElement("animations");
if (animations_elem)
{
for (TiXmlElement* animation_elem = animations_elem->FirstChildElement("animation"); animation_elem != NULL; animation_elem = animation_elem->NextSiblingElement("animation"))
{
StdCopyStrBuf name(skeleton->RequireStrAttribute(animation_elem, "name"));
if (Skeleton->Animations.find(name) != Skeleton->Animations.end())
skeleton->Error(FormatString("There is already an animation with name '%s'", name.getData()), animation_elem);
StdMeshAnimation& animation = Skeleton->Animations.insert(std::make_pair(name, StdMeshAnimation())).first->second;
animation.Name = name;
animation.Length = skeleton->RequireFloatAttribute(animation_elem, "length");
animation.Tracks.resize(Skeleton->GetNumBones());
animation.OriginSkeleton = &(*Skeleton);
TiXmlElement* tracks_elem = skeleton->RequireFirstChild(animation_elem, "tracks");
for (TiXmlElement* track_elem = tracks_elem->FirstChildElement("track"); track_elem != NULL; track_elem = track_elem->NextSiblingElement("track"))
{
const char* bone_name = skeleton->RequireStrAttribute(track_elem, "bone");
StdMeshBone* bone = NULL;
for (unsigned int i = 0; !bone && i < Skeleton->GetNumBones(); ++i)
if (Skeleton->Bones[i]->Name == bone_name)
bone = Skeleton->Bones[i];
if (!bone) skeleton->Error(FormatString("There is no such bone with name '%s'", bone_name), track_elem);
if (animation.Tracks[bone->Index] != NULL) skeleton->Error(FormatString("There is already a track for bone '%s' in animation '%s'", bone_name, animation.Name.getData()), track_elem);
StdMeshTrack* track = new StdMeshTrack;
animation.Tracks[bone->Index] = track;
TiXmlElement* keyframes_elem = skeleton->RequireFirstChild(track_elem, "keyframes");
for (TiXmlElement* keyframe_elem = keyframes_elem->FirstChildElement("keyframe"); keyframe_elem != NULL; keyframe_elem = keyframe_elem->NextSiblingElement("keyframe"))
{
float time = skeleton->RequireFloatAttribute(keyframe_elem, "time");
StdMeshKeyFrame& frame = track->Frames[time];
TiXmlElement* translate_elem = keyframe_elem->FirstChildElement("translate");
TiXmlElement* rotate_elem = keyframe_elem->FirstChildElement("rotate");
TiXmlElement* scale_elem = keyframe_elem->FirstChildElement("scale");
StdMeshVector d, s, r;
d.x = d.y = d.z = 0.0f;
s = StdMeshVector::UnitScale();
r.x = r.y = 0.0f; r.z = 1.0f;
float angle = 0.0f;
if (translate_elem)
{
d.x = skeleton->RequireFloatAttribute(translate_elem, "x");
d.y = skeleton->RequireFloatAttribute(translate_elem, "y");
d.z = skeleton->RequireFloatAttribute(translate_elem, "z");
}
if (rotate_elem)
{
TiXmlElement* axis_elem = skeleton->RequireFirstChild(rotate_elem, "axis");
angle = skeleton->RequireFloatAttribute(rotate_elem, "angle");
r.x = skeleton->RequireFloatAttribute(axis_elem, "x");
r.y = skeleton->RequireFloatAttribute(axis_elem, "y");
r.z = skeleton->RequireFloatAttribute(axis_elem, "z");
}
if (scale_elem)
{
s.x = skeleton->RequireFloatAttribute(scale_elem, "x");
s.y = skeleton->RequireFloatAttribute(scale_elem, "y");
s.z = skeleton->RequireFloatAttribute(scale_elem, "z");
}
frame.Transformation.scale = s;
frame.Transformation.rotate = StdMeshQuaternion::AngleAxis(angle, r);
frame.Transformation.translate = bone->InverseTransformation.rotate * (bone->InverseTransformation.scale * d);
frame.Transformation = OgreToClonk::TransformTransformation(frame.Transformation);
}
}
}
}
// is there even any xml file that we load from?
// it looks like this could never work: if the mesh has no skeleton, then the code below will fail because of a null pointer...
// Apply parent transformation to each bone transformation. We need to
// do this late since animation keyframe computation needs the bone
// transformations, not bone+parent.
for (unsigned int i = 0; i < Skeleton->GetNumBones(); ++i)
{
// Apply parent transformation
if (Skeleton->Bones[i]->Parent)
Skeleton->Bones[i]->Transformation = Skeleton->Bones[i]->Parent->Transformation * OgreToClonk::TransformTransformation(Skeleton->Bones[i]->Transformation);
else
Skeleton->Bones[i]->Transformation = OgreToClonk::TransformTransformation(Skeleton->Bones[i]->Transformation);
// Update inverse
Skeleton->Bones[i]->InverseTransformation = StdMeshTransformation::Inverse(Skeleton->Bones[i]->Transformation);
}
StoreSkeleton(groupname, filename, Skeleton);
}
bool Skeleton_Builder::BuildSkeleton(Skeleton& newSkel, Ogre::Vector3 pos)
{
if (mLeg == LegType::None || mHeight <= 10) // check to ensure all attribs required
return false;
float neck_height;
float torso_height;
float Leg_height;
float tail_height;
//create base node for skeleton
mBase = mSceneMgr->getRootSceneNode()->createChildSceneNode(pos);
// set block area heights
// if longneck neck/legs ratio = 2:1
// if shortneck neck/legs ratio = 1:2
// if upright torso 40%
// else torso 25%
if (IsUpright())
{
auto onepercent = mHeight / 100;
Leg_height = onepercent * 40;
if (IsLongNeck())
{
neck_height = onepercent * 40;
torso_height = onepercent * 20;
}
else
{
neck_height = onepercent * 20;
torso_height = onepercent * 40;
}
}
else
{
auto onepercent = mHeight / 100;
torso_height = onepercent * 25;
if (IsLongNeck())
{
neck_height = onepercent * 50;
Leg_height = onepercent * 25;
}
else
{
neck_height = onepercent * 25;
Leg_height = onepercent * 50;
}
}
if (IsLongTail())
{
auto onepercent = mHeight / 100;
tail_height = onepercent * 50;
}
else
{
auto onepercent = mHeight / 100;
tail_height = onepercent * 25;
}
//set block area widths
float neck_width;
float torso_width;
float Leg_width;
float Arm_width;
auto onepercentW = mWidth / 100;
// arms take 35% of width i.e 17.5% each side if needed
torso_width = onepercentW * 65;
Leg_width = onepercentW * 25; // ensures gaps between legs like realistic bipeds
Arm_width = onepercentW * 17.5f;
neck_width = onepercentW * 35;
//build the torso here
Bone* CentreTorso = new Bone();
if (IsUpright())
{
Bone* TopTorso = new Bone();
Bone* BotTorso = new Bone();
//build 3 torso pieces
torso_height /= 3;
//build centre piece
mBuilder->SetDimensions(torso_width, torso_height, mDepth);
mBuilder->SetRelativePosition(Ogre::Vector3::ZERO, Ogre::Quaternion::IDENTITY,*mBase);
if (!mBuilder->BuildBone(*CentreTorso,Bone::BoneType::Torso))
return false;
//build bot piece
mBuilder->SetDimensions(torso_width, torso_height, mDepth);
mBuilder->SetRelativePosition(Ogre::Vector3(0, -(torso_height), 0), Ogre::Quaternion::IDENTITY, *CentreTorso->GetNode());
if (!mBuilder->BuildBone(*BotTorso,Bone::BoneType::Hip))
return false;
//build top piece
mBuilder->SetDimensions(torso_width, torso_height, mDepth);
mBuilder->SetRelativePosition(Ogre::Vector3(0, (torso_height), 0), Ogre::Quaternion::IDENTITY, *CentreTorso->GetNode());
if (!mBuilder->BuildBone(*TopTorso, Bone::BoneType::Shoulder))
return false;
mShouldernode = TopTorso;
mHipNode = BotTorso;
mBones.push_back(CentreTorso);
mBones.push_back(TopTorso);
mBones.push_back(BotTorso);
//add muscles
mMuscleBuilder->CreateMuscle(CentreTorso, TopTorso, mMuscles);
mMuscleBuilder->CreateMuscle(CentreTorso, BotTorso, mMuscles);
btTransform TransA;
btTransform TransB;
btVector3 JointPos; // world position of transform
/*add constraints*/
// JOINT POS IS THE POSITION ON THE EDGE OF THE BONE ATTACH I.E BONE POS + EDGE OF BONE IN DIRECTION TO NEXT
JointPos = Utils::OgreBTVector(CentreTorso->GetNode()->_getDerivedPosition() + (torso_height / 2));
SetJointTransform(TransA, TransB, JointPos, CentreTorso, TopTorso, btVector3(0, btScalar(-M_PI_2), 0));
btConeTwistConstraint* TopSpine = new btConeTwistConstraint(*CentreTorso->GetRigidBody(), *TopTorso->GetRigidBody(), TransA, TransB);
TopSpine->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4));
JointPos = Utils::OgreBTVector(CentreTorso->GetNode()->_getDerivedPosition() - (torso_height / 2));
SetJointTransform(TransA, TransB, JointPos, CentreTorso, BotTorso, btVector3(0, btScalar(-M_PI_2), 0));
btConeTwistConstraint* BotSpine = new btConeTwistConstraint(*CentreTorso->GetRigidBody(), *BotTorso->GetRigidBody(), TransA, TransB);
BotSpine->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4));
mConstraints.push_back(TopSpine);
mConstraints.push_back(BotSpine);
}
else
{
//Bone* CentreTorso = new Bone();
//build centre piece
mBuilder->SetDimensions(torso_width, torso_height, mDepth);
mBuilder->SetRelativePosition(Ogre::Vector3::ZERO, Ogre::Quaternion::IDENTITY, *mBase);
if (!mBuilder->BuildBone(*CentreTorso,Bone::BoneType::HipShoulder))
return false;
mBones.push_back(CentreTorso);
mShouldernode = CentreTorso;
mHipNode = CentreTorso;
}
if (mFixed)
CentreTorso->GetRigidBody()->setMassProps(0, btVector3(0, 0, 0));
//if (!BuildArm(Arm_width, Leg_height, torso_width) || !BuildNeck(neck_width, neck_height, torso_height) || !BuildLeg(Leg_width, Leg_height, torso_width, torso_height))
// return false;
if (!mHidearms)
if (!BuildArm(Arm_width, torso_height, torso_width))
return false;
if (!mHideneck)
if (!BuildNeck(neck_width, neck_height, torso_height))
return false;
if (!mHidelegs)
if (!BuildLeg(Leg_width, Leg_height, torso_width, torso_height))
return false;
if (IsShortTail() || IsLongTail())
if(!BuildTail(neck_width,tail_height, torso_height))
return false;
newSkel = Skeleton(mBones,mMuscles, mConstraints, mBase, mWorld);
ClearData();
return true;
}
