bool LWBone::collectData()
{
LWItemInfo *itemInfo = Manager::getSingleton()->getItemInfo();
LWBoneInfo *boneInfo = Manager::getSingleton()->getBoneInfo();
if((itemInfo == 0) || (boneInfo == 0))
return false;
if(validateBone() == false)
return false;
name = itemInfo->name(lwId);
parentId = itemInfo->parent(lwId);
if(parentId && !validateBoneProperties(parentId))
parentId = 0;
if((parentId) && (itemInfo->type(parentId) == LWI_BONE))
parentName = itemInfo->name(parentId);
if(const char *cMap = boneInfo->weightMap(lwId))
weightMap = cMap;
// Transform
FBMatrix boneTm = LWTransforms::GetBoneRestTransform(lwId);
double inner = 0;
double outer = 0;
boneInfo->limits(lwId, &inner, &outer);
float boneRadius = float(outer);
double boneLength = boneInfo->restLength(lwId);
double scale = LWTransforms::GetBoneScale(lwId);
// Bone position
boneTm.Set(12, boneTm.Get(12) * scale);
boneTm.Set(13, boneTm.Get(13) * scale);
boneTm.Set(14, boneTm.Get(14) * scale);
// Exporter instance
Bone bone(name, parentName);
bone.setRestTransform(boneTm);
bone.setRestLength(boneLength * scale);
bone.setRadius(boneRadius);
// Store
exporterId = Manager::getSingleton()->getExporter()->addBone(bone);
return true;
}
//-------------------------------------------------------------------------
void
AFile::addTo( Ogre::SkeletonPtr skeleton, const String& name ) const
{
if( skeleton->hasAnimation( name) ) return;
Ogre::Real length( ( m_frames.size() - 1 ) * FRAME_DURATION );
Ogre::Animation *anim( skeleton->createAnimation(name, length ));
uint16 track_handle( 0 );
Ogre::Bone* bone( skeleton->getBone( "root" ) );
Ogre::NodeAnimationTrack* track;
track = anim->createNodeTrack( track_handle++, bone );
Ogre::Real time;
size_t index( 0 );
for( FrameList::const_iterator frame( m_frames.begin())
;frame != m_frames.end(); ++frame )
{
time = (index++) * FRAME_DURATION;
Ogre::TransformKeyFrame* key_frame( track->createNodeKeyFrame( time ) );
key_frame->setTranslate( frame->root_translation );
setFrameRotation( key_frame, frame->root_rotation );
}
for( uint32 i(0); i < m_bone_count; ++i )
{
if (i + 1 >= skeleton->getNumBones())
{
// TODO: Figure out why this happens/fix it
LOG_ERROR("Bone " + std::to_string(i + 1) + " is out of bounds " + std::to_string(skeleton->getNumBones()) + " for: " + name + " in: " + skeleton->getName());
}
else
{
bone = skeleton->getBone(i + 1);
track = anim->createNodeTrack(track_handle++, bone);
time = 0;
for (FrameList::const_iterator frame(m_frames.begin())
; frame != m_frames.end(); ++frame)
{
const Ogre::Vector3& rot(frame->bone_rotations[i]);
Ogre::TransformKeyFrame* key_frame(track->createNodeKeyFrame(time));
setFrameRotation(key_frame, rot);
time += FRAME_DURATION;
}
}
}
}
SkeletonSprite::SkeletonSprite(const String& filename) : Sprite(NULL) {
// Cargamos los datos del XML
SkeletonData* data = new SkeletonData(filename);
root = new Bone("world", NULL, 0, 0, 0, 0);
// Generamos huesos
for ( uint32 i = 0; i < data->GetBoneDatas().Size(); i++ ) {
// Obtenemos hueso
const BoneData& boneData = data->GetBoneDatas()[i];
// Obtenemos padre del hueso
Bone* parent = root;
if ( boneData.GetParentName() != "world" )
parent = root->FindChild(boneData.GetParentName());
// Obtenemos imagen
Image* image = ResourceManager::Instance().LoadImage(filename.ExtractDir() + "/" + boneData.GetImageFilename());
// Creamos hueso
Bone bone(boneData.GetId(), image, boneData.GetPivotX(), boneData.GetPivotY(), boneData.GetHandleX(), boneData.GetHandleY());
// Aniadimos frames
for ( uint32 i = 0; i < boneData.GetFrames().Size(); i++ )
bone.AddFrame(boneData.GetFrames()[i]);
// Aniadimos hueso
parent->AddChild(bone);
}
// Establecemos el rango de la animacion
const Bone* bone = root->GetChild(0);
int32 lastframe = 0;
for ( uint32 index = 0; index < bone->CountFrames(); index++ ) {
lastframe = max(lastframe, bone->GetFrame(index)->GetId());
}
SetFrameRange(0, lastframe);
// Eliminamos los datos cargados del XML
delete data;
}
/*
Recurses through the scene and creates Bones from the eSkeleton nodes found
*/
void SceneImporter::extractBonesRecursive(FbxNode* pNode)
{
// Bake material and hook as user data.
FbxNodeAttribute* lNodeAttribute = pNode->GetNodeAttribute();
if (lNodeAttribute)
{
// Bake mesh as VBO(vertex buffer object) into GPU.
if (lNodeAttribute->GetAttributeType() == FbxNodeAttribute::eSkeleton)
{
// Add a reference to the Bone object to the node so any meshes that have clusters using this bone can obtain the Bone reference
shared_ptr<Bone> bone(new Bone(pNode));
_bones.push_back(bone);
pNode->SetUserDataPtr(&_bones.back());
}
}
const int lChildCount = pNode->GetChildCount();
for (int lChildIndex = 0; lChildIndex < lChildCount; ++lChildIndex)
{
extractBonesRecursive(pNode->GetChild(lChildIndex));
}
}
Frame AnimationController::DoGetFrame(const Frame &, const Frame &) const
{
Frame frame;
if (position) frame.position = position.Get(time);
if (orientation) frame.orientation = orientation.Get(time);
if (normal) frame.normal = normal.Get(time);
if (scale) frame.scale = scale.Get(time);
if (texCoord) frame.texCoord = texCoord.Get(time);
if (ambient) frame.ambient = ambient.Get(time);
if (diffuse) frame.diffuse = diffuse.Get(time);
if (specular) frame.specular = specular.Get(time);
if (attenuation) frame.attenuation = attenuation.Get(time);
if (cutoff) frame.cutoff = cutoff.Get(time);
if (depth) frame.depth = depth.Get(time);
if (exposure) frame.exposure = exposure.Get(time);
if (falloff) frame.falloff = falloff.Get(time);
if (fov) frame.fov = fov.Get(time);
if (opacity) frame.opacity = opacity.Get(time);
if (range) frame.range = range.Get(time);
if (size) frame.size = size.Get(time);
if (volume) frame.volume = volume.Get(time);
for (Bones::const_iterator bone(bones.begin()); bone != bones.end(); ++bone)
{
Frame::Bone &frameBone(frame.bones[bone->name]);
if (bone->position) frameBone.position = bone->position.Get(time);
if (bone->orientation) frameBone.orientation = bone->orientation.Get(time);
if (bone->scale) frameBone.scale = bone->scale.Get(time);
}
for (Vertices::const_iterator vertex(vertices.begin()); vertex != vertices.end(); ++vertex)
{
Frame::Vertex &frameVertex(frame.vertices[vertex->index]);
if (vertex->position) frameVertex.position = vertex->position.Get(time);
if (vertex->normal) frameVertex.normal = vertex->normal.Get(time);
if (vertex->texCoord) frameVertex.texCoord = vertex->texCoord.Get(time);
}
return frame;
}
Window::Window(int width, int height, int bpp) :
width(width), height(height), bpp(bpp), quit(false), isActive(true),
lightPos(Vec4(2.0, 5.0, 0.5, 1.0)), topView(false),
from(new Vec3(-2, 4.0, 8)), at(new Vec3(0.0, 0.0, -1.0)),
up(new Vec3(0, 1, 0)), camera(from, at, up)
{
initSDL();
initGL();
AC3DConverter a;
model = ModelPtr(a.loadAC3DModel("../data/cylinder.ac"));
BonePtr bone(model->getBone());
bone->setPos(Vec3(-3.0, 0.0, 0.0));
JointPtr joint = bone->getJoint();
joint->setPos(Vec3(0.0, 0.0, 0.0));
BonePtr b(new Bone);
joint->setBone(b);
BonePtr bone2 = joint->getBone();
JointPtr joint2 = bone2->getJoint();
joint2->setPos(Vec3(3.0, 0.0, 0.0));
bone->setTransform(vr::Matrix::translate(0.0, 2.0, 0.0));
bone->getJoint()->setTransform(vr::Matrix::translate(0.0, -2.0, 0.0));
bone2->setTransform(vr::Matrix::translate(0.0, 2.0, 0.0));
model->calcWeight();
ShaderPtr shader(new Shader("shaders/skin.vert", "shaders/skin.frag"));
program = ProgramPtr(new Program());
program->addShaders(shader);
tex = TexturePtr(new Texture());
tex->createTextureARB("../data/alpha.png", GL_TEXTURE0_ARB);
uniformTex = Uniform1iPtr(new Uniform<int, 1>("ualpha", program->getId()));
uniformTex->setValue(0);
program->addUniform(uniformTex);
texFur = TexturePtr(new Texture());
texFur->createTextureARB("../data/Fur1.png", GL_TEXTURE1_ARB);
uniformTexFur = Uniform1iPtr(new Uniform<int, 1>("ufur", program->getId()));
uniformTexFur->setValue(1);
uniformTexFur->apply();
program->addUniform(uniformTexFur);
uniformPass = Uniform1iPtr(new Uniform<int, 1>("upass", program->getId()));
program->addUniform(uniformPass);
uniformMatrices = UniformMatrix3fvPtr(new Uniform<vr::Matrix, 3>("boneMatrices[0]", program->getId()));
program->addUniform(uniformMatrices);
rotation = 0.0f;
for (unsigned int i = 0; i < 20; i++)
{
pressedKeys[i] = false;
}
}
void Window::render()
{
static GLint t0 = 0;
static GLint T0 = 0;
static GLint T0a = 0;
static GLint frames = 0;
GLint t = SDL_GetTicks();
frameTime = t - t0;
glDepthMask(GL_TRUE);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glPushAttrib(GL_ALL_ATTRIB_BITS);
glLoadIdentity();
camera.setView(width, height);
moveCamera();
glLoadMatrixf(Matrix::lookAt(*from, *at, *up).ptr());
tex->activate();
texFur->activate();
program->apply();
glColor3f(1.0f, 1.0f, 1.0f);
uniformPass->setValue(-1);
uniformPass->apply();
model->draw(program->getId());
glDepthMask(GL_FALSE);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE);
glBlendEquation(GL_FUNC_ADD);
for(int i=1; i<20; ++i)
{
uniformPass->setValue(i);
uniformPass->apply();
model->draw(program->getId(),tex->getTexID());
}
glDisable(GL_TEXTURE_2D);
glDisable(GL_BLEND);
glDepthMask(GL_TRUE);
glColor3f(1.0f, 0.0f, 0.0f);
BonePtr bone1(model->getBone());
BonePtr bone2 = bone1->getJoint()->getBone();
glUseProgramObjectARB(0);
glPopAttrib();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glViewport(0, 0, (GLsizei) width, (GLsizei) height);
frames++;
{
t0 = t;
if (t - T0 >= 1000) {
GLfloat seconds = (t - T0) / 1000.0;
GLfloat fps = frames / seconds;
info.str("");
info << "FPS: " << fixed << setprecision(0) << fps << endl;
T0 = t;
frames = 0;
}
if (t - T0a >= 10)
{
rotation += 0.01;
rotation2 += 0.03;
if(rotation > 2*M_PI)
rotation = 0.0f;
if(rotation2 > 2*M_PI)
rotation2 = 0.0f;
T0a = t;
}
}
displayText(10, height-10, info);
BonePtr bone(model->getBone());
bone->setTransform(vr::Matrix::translate(0.0, cos(rotation)*2.0, sin(rotation)*2.0));
bone->getJoint()->setTransform(vr::Matrix::translate(0.0, -2.0, 0.0));
bone->getJoint()->getBone()->setTransform( vr::Matrix::translate(0.0, cos(rotation2)*3.0, -2.0) );
uniformMatrices->setValue(bone->getTransform(),
bone->getJoint()->getTransform(),
bone->getJoint()->getBone()->getJoint()->getTransform());
SDL_GL_SwapBuffers();
}
void Kinect::run()
{
while ( mCapture ) {
if ( mSensor != 0 ) {
// Get elapsed time to calculate frame rate
double time = getElapsedSeconds();
//////////////////////////////////////////////////////////////////////////////////////////////
if ( mDeviceOptions.isDepthEnabled() && mDepthStreamHandle != 0 && !mNewDepthSurface ) {
_NUI_IMAGE_FRAME imageFrame;
long hr = mSensor->NuiImageStreamGetNextFrame( mDepthStreamHandle, WAIT_TIME, &imageFrame );
if ( FAILED( hr ) ) {
error( hr );
} else {
INuiFrameTexture * texture = imageFrame.pFrameTexture;
_NUI_LOCKED_RECT lockedRect;
hr = texture->LockRect( 0, &lockedRect, 0, 0 );
if ( FAILED( hr ) ) {
error( hr );
}
if ( lockedRect.Pitch == 0 ) {
trace( "Invalid buffer length received" );
} else {
pixelToDepthSurface( (uint16_t*)lockedRect.pBits );
}
hr = mSensor->NuiImageStreamReleaseFrame( mDepthStreamHandle, &imageFrame );
if ( FAILED( hr ) ) {
error( hr );
}
mFrameRateDepth = (float)( 1.0 / ( time - mReadTimeDepth ) );
mReadTimeDepth = time;
mUserCount = 0;
for ( uint32_t i = 0; i < NUI_SKELETON_COUNT; i++ ) {
if ( mActiveUsers[ i ] ) {
mUserCount++;
}
}
mNewDepthSurface = true;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
if ( mDeviceOptions.isSkeletonTrackingEnabled() && mIsSkeletonDevice && !mNewSkeletons ) {
_NUI_SKELETON_FRAME skeletonFrame;
long hr = mSensor->NuiSkeletonGetNextFrame( WAIT_TIME, &skeletonFrame );
if ( FAILED( hr ) ) {
error( hr );
} else {
bool foundSkeleton = false;
for ( int32_t i = 0; i < NUI_SKELETON_COUNT; i++ ) {
mSkeletons.at( i ).clear();
NUI_SKELETON_TRACKING_STATE trackingState = skeletonFrame.SkeletonData[ i ].eTrackingState;
if ( trackingState == NUI_SKELETON_TRACKED || trackingState == NUI_SKELETON_POSITION_ONLY ) {
if ( !foundSkeleton ) {
_NUI_TRANSFORM_SMOOTH_PARAMETERS transform = kTransformParams[ mTransform ];
hr = mSensor->NuiTransformSmooth( &skeletonFrame, &transform );
if ( FAILED( hr ) ) {
error( hr );
}
foundSkeleton = true;
}
// Flip X when flipping the image.
if ( mFlipped ) {
( skeletonFrame.SkeletonData + i )->Position.x *= -1.0f;
for ( int32_t j = 0; j < (int32_t)NUI_SKELETON_POSITION_COUNT; j++ ) {
( skeletonFrame.SkeletonData + i )->SkeletonPositions[ j ].x *= -1.0f;
}
}
_NUI_SKELETON_BONE_ORIENTATION bones[ NUI_SKELETON_POSITION_COUNT ];
hr = NuiSkeletonCalculateBoneOrientations( skeletonFrame.SkeletonData + i, bones );
if ( FAILED( hr ) ) {
error( hr );
}
for ( int32_t j = 0; j < (int32_t)NUI_SKELETON_POSITION_COUNT; j++ ) {
Bone bone( *( ( skeletonFrame.SkeletonData + i )->SkeletonPositions + j ), *( bones + j ) );
( mSkeletons.begin() + i )->insert( std::make_pair<JointName, Bone>( (JointName)j, bone ) );
}
}
}
mFrameRateSkeletons = (float)( 1.0 / ( time - mReadTimeSkeletons ) );
mReadTimeSkeletons = time;
mNewSkeletons = true;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
if ( mDeviceOptions.isVideoEnabled() && mVideoStreamHandle != 0 && !mNewVideoSurface ) {
_NUI_IMAGE_FRAME imageFrame;
long hr = mSensor->NuiImageStreamGetNextFrame( mVideoStreamHandle, WAIT_TIME, &imageFrame );
if ( FAILED( hr ) ) {
error( hr );
} else {
INuiFrameTexture * texture = imageFrame.pFrameTexture;
_NUI_LOCKED_RECT lockedRect;
hr = texture->LockRect( 0, &lockedRect, 0, 0 );
if ( FAILED( hr ) ) {
error( hr );
}
if ( lockedRect.Pitch != 0 ) {
pixelToVideoSurface( (uint8_t *)lockedRect.pBits );
} else {
trace( "Invalid buffer length received." );
}
hr = mSensor->NuiImageStreamReleaseFrame( mVideoStreamHandle, &imageFrame );
if ( FAILED( hr ) ) {
error( hr );
}
mFrameRateVideo = (float)( 1.0 / ( time - mReadTimeVideo ) );
mReadTimeVideo = time;
mNewVideoSurface = true;
}
}
}
Sleep( 8 );
}
// Return to join thread
return;
}
VertexData TransformUnit::ReadVertex(VertexReader& vreader)
{
VertexData vertex;
float pos[3];
// VertexDecoder normally scales z, but we want it unscaled.
vreader.ReadPosThroughZ16(pos);
if (!gstate.isModeClear() && gstate.isTextureMapEnabled() && vreader.hasUV()) {
float uv[2];
vreader.ReadUV(uv);
vertex.texturecoords = Vec2<float>(uv[0], uv[1]);
}
if (vreader.hasNormal()) {
float normal[3];
vreader.ReadNrm(normal);
vertex.normal = Vec3<float>(normal[0], normal[1], normal[2]);
if (gstate.areNormalsReversed())
vertex.normal = -vertex.normal;
}
if (vertTypeIsSkinningEnabled(gstate.vertType) && !gstate.isModeThrough()) {
float W[8] = { 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f };
vreader.ReadWeights(W);
Vec3<float> tmppos(0.f, 0.f, 0.f);
Vec3<float> tmpnrm(0.f, 0.f, 0.f);
for (int i = 0; i < vertTypeGetNumBoneWeights(gstate.vertType); ++i) {
Mat3x3<float> bone(&gstate.boneMatrix[12*i]);
tmppos += (bone * ModelCoords(pos[0], pos[1], pos[2]) + Vec3<float>(gstate.boneMatrix[12*i+9], gstate.boneMatrix[12*i+10], gstate.boneMatrix[12*i+11])) * W[i];
if (vreader.hasNormal())
tmpnrm += (bone * vertex.normal) * W[i];
}
pos[0] = tmppos.x;
pos[1] = tmppos.y;
pos[2] = tmppos.z;
if (vreader.hasNormal())
vertex.normal = tmpnrm;
}
if (vreader.hasColor0()) {
float col[4];
vreader.ReadColor0(col);
vertex.color0 = Vec4<int>(col[0]*255, col[1]*255, col[2]*255, col[3]*255);
} else {
vertex.color0 = Vec4<int>(gstate.getMaterialAmbientR(), gstate.getMaterialAmbientG(), gstate.getMaterialAmbientB(), gstate.getMaterialAmbientA());
}
if (vreader.hasColor1()) {
float col[3];
vreader.ReadColor1(col);
vertex.color1 = Vec3<int>(col[0]*255, col[1]*255, col[2]*255);
} else {
vertex.color1 = Vec3<int>(0, 0, 0);
}
if (!gstate.isModeThrough()) {
vertex.modelpos = ModelCoords(pos[0], pos[1], pos[2]);
vertex.worldpos = WorldCoords(TransformUnit::ModelToWorld(vertex.modelpos));
ModelCoords viewpos = TransformUnit::WorldToView(vertex.worldpos);
vertex.clippos = ClipCoords(TransformUnit::ViewToClip(viewpos));
if (gstate.isFogEnabled()) {
float fog_end = getFloat24(gstate.fog1);
float fog_slope = getFloat24(gstate.fog2);
// Same fixup as in ShaderManagerGLES.cpp
if (my_isnanorinf(fog_end)) {
// Not really sure what a sensible value might be, but let's try 64k.
fog_end = std::signbit(fog_end) ? -65535.0f : 65535.0f;
}
if (my_isnanorinf(fog_slope)) {
fog_slope = std::signbit(fog_slope) ? -65535.0f : 65535.0f;
}
vertex.fogdepth = (viewpos.z + fog_end) * fog_slope;
} else {
vertex.fogdepth = 1.0f;
}
vertex.screenpos = ClipToScreenInternal(vertex.clippos, &outside_range_flag);
if (vreader.hasNormal()) {
vertex.worldnormal = TransformUnit::ModelToWorldNormal(vertex.normal);
// TODO: Isn't there a flag that controls whether to normalize the normal?
vertex.worldnormal /= vertex.worldnormal.Length();
} else {
vertex.worldnormal = Vec3<float>(0.0f, 0.0f, 1.0f);
}
Lighting::Process(vertex, vreader.hasColor0());
} else {
vertex.screenpos.x = (int)(pos[0] * 16) + gstate.getOffsetX16();
vertex.screenpos.y = (int)(pos[1] * 16) + gstate.getOffsetY16();
vertex.screenpos.z = pos[2];
vertex.clippos.w = 1.f;
vertex.fogdepth = 1.f;
}
return vertex;
}
static VertexData ReadVertex(VertexReader& vreader)
{
VertexData vertex;
float pos[3];
// VertexDecoder normally scales z, but we want it unscaled.
vreader.ReadPosZ16(pos);
if (!gstate.isModeClear() && gstate.isTextureMapEnabled() && vreader.hasUV()) {
float uv[2];
vreader.ReadUV(uv);
vertex.texturecoords = Vec2<float>(uv[0], uv[1]);
}
if (vreader.hasNormal()) {
float normal[3];
vreader.ReadNrm(normal);
vertex.normal = Vec3<float>(normal[0], normal[1], normal[2]);
if (gstate.areNormalsReversed())
vertex.normal = -vertex.normal;
}
if (gstate.isSkinningEnabled() && !gstate.isModeThrough()) {
float W[8] = { 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f };
vreader.ReadWeights(W);
Vec3<float> tmppos(0.f, 0.f, 0.f);
Vec3<float> tmpnrm(0.f, 0.f, 0.f);
for (int i = 0; i < gstate.getNumBoneWeights(); ++i) {
Mat3x3<float> bone(&gstate.boneMatrix[12*i]);
tmppos += W[i] * (bone * ModelCoords(pos[0], pos[1], pos[2]) + Vec3<float>(gstate.boneMatrix[12*i+9], gstate.boneMatrix[12*i+10], gstate.boneMatrix[12*i+11]));
if (vreader.hasNormal())
tmpnrm += W[i] * (bone * vertex.normal);
}
pos[0] = tmppos.x;
pos[1] = tmppos.y;
pos[2] = tmppos.z;
if (vreader.hasNormal())
vertex.normal = tmpnrm;
}
if (vreader.hasColor0()) {
float col[4];
vreader.ReadColor0(col);
vertex.color0 = Vec4<int>(col[0]*255, col[1]*255, col[2]*255, col[3]*255);
} else {
vertex.color0 = Vec4<int>(gstate.getMaterialAmbientR(), gstate.getMaterialAmbientG(), gstate.getMaterialAmbientB(), gstate.getMaterialAmbientA());
}
if (vreader.hasColor1()) {
float col[3];
vreader.ReadColor1(col);
vertex.color1 = Vec3<int>(col[0]*255, col[1]*255, col[2]*255);
} else {
vertex.color1 = Vec3<int>(0, 0, 0);
}
if (!gstate.isModeThrough()) {
vertex.modelpos = ModelCoords(pos[0], pos[1], pos[2]);
vertex.worldpos = WorldCoords(TransformUnit::ModelToWorld(vertex.modelpos));
vertex.clippos = ClipCoords(TransformUnit::ViewToClip(TransformUnit::WorldToView(vertex.worldpos)));
vertex.screenpos = ClipToScreenInternal(vertex.clippos);
if (vreader.hasNormal()) {
vertex.worldnormal = TransformUnit::ModelToWorld(vertex.normal) - Vec3<float>(gstate.worldMatrix[9], gstate.worldMatrix[10], gstate.worldMatrix[11]);
vertex.worldnormal /= vertex.worldnormal.Length(); // TODO: Shouldn't be necessary..
}
Lighting::Process(vertex);
} else {
vertex.screenpos.x = (u32)pos[0] * 16 + gstate.getOffsetX16();
vertex.screenpos.y = (u32)pos[1] * 16 + gstate.getOffsetY16();
vertex.screenpos.z = pos[2];
vertex.clippos.w = 1.f;
}
return vertex;
}
void Device::run()
{
HANDLE events[ 4 ];
events[ 0 ] = mColorEvent;
events[ 1 ] = mDepthEvent;
events[ 2 ] = mSkeletonEvent;
while ( mCapture ) {
if ( mSensor != 0 ) {
double time = getElapsedSeconds();
WaitForMultipleObjects( sizeof( events ) / sizeof( events[ 0 ]), events, 0, WAIT_TIME );
const NUI_IMAGE_FRAME* frameColor = 0;
const NUI_IMAGE_FRAME* frameDepth = 0;
NUI_SKELETON_FRAME frameSkeleton = { 0 };
bool readColor = !mNewColorSurface;
bool readDepth = !mNewDepthSurface;
bool readSkeleton = !mNewSkeletons;
/*if ( mDeviceOptions.isFrameSyncEnabled() && mDeviceOptions.isColorEnabled() && mDeviceOptions.isDepthEnabled() ) {
if ( readColor != readDepth ) {
readColor = false;
readDepth = false;
}
readSkeleton = readDepth;
}*/
readColor = readColor && mDeviceOptions.isColorEnabled();
readDepth = readDepth && mDeviceOptions.isDepthEnabled();
readSkeleton = readSkeleton && mDeviceOptions.isSkeletonTrackingEnabled();
//////////////////////////////////////////////////////////////////////////////////////////////
if ( readDepth && WAIT_OBJECT_0 == WaitForSingleObject( mDepthEvent, 0 ) ) {
if ( SUCCEEDED( NuiImageStreamGetNextFrame( mDepthStreamHandle, 0, &frameDepth ) ) &&
frameDepth != 0 && frameDepth->pFrameTexture != 0 ) {
mDepthTimeStamp = frameDepth->liTimeStamp.QuadPart;
INuiFrameTexture* texture = frameDepth->pFrameTexture;
_NUI_LOCKED_RECT lockedRect;
long hr = texture->LockRect( 0, &lockedRect, 0, 0 );
if ( FAILED( hr ) ) {
error( hr );
}
if ( lockedRect.Pitch == 0 ) {
console() << "Invalid buffer length received" << endl;
} else {
pixelToDepthSurface( (uint16_t*)lockedRect.pBits );
}
hr = NuiImageStreamReleaseFrame( mDepthStreamHandle, frameDepth );
if ( FAILED( hr ) ) {
error( hr );
}
mUserCount = 0;
for ( uint32_t i = 0; i < NUI_SKELETON_COUNT; ++i ) {
if ( mActiveUsers[ i ] ) {
++mUserCount;
}
}
mNewDepthSurface = true;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
if ( readColor && WAIT_OBJECT_0 == WaitForSingleObject( mColorEvent, 0 ) ) {
if ( SUCCEEDED( NuiImageStreamGetNextFrame( mColorStreamHandle, 0, &frameColor ) ) &&
frameColor != 0 && frameColor->pFrameTexture != 0 ) {
INuiFrameTexture* texture = frameColor->pFrameTexture;
_NUI_LOCKED_RECT lockedRect;
long hr = texture->LockRect( 0, &lockedRect, 0, 0 );
if ( FAILED( hr ) ) {
error( hr );
}
if ( lockedRect.Pitch != 0 ) {
pixelToColorSurface( (uint8_t*)lockedRect.pBits );
/*if ( mDeviceOptions.isFrameSyncEnabled() ) {
mColorFrames.push_back( ColorFrame( mColorSurface, frameColor->liTimeStamp.QuadPart ) );
if ( mColorFrames.size() > 10 ) {
mColorFrames.erase( mColorFrames.begin() );
}
}*/
} else {
console() << "Invalid buffer length received." << endl;
}
hr = NuiImageStreamReleaseFrame( mColorStreamHandle, frameColor );
if ( FAILED( hr ) ) {
error( hr );
}
mNewColorSurface = true;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
if ( readSkeleton && WAIT_OBJECT_0 == WaitForSingleObject( mSkeletonEvent, 0 ) ) {
long hr = NuiSkeletonGetNextFrame( 0, &frameSkeleton );
if ( SUCCEEDED( hr ) ) {
bool foundSkeleton = false;
for ( int32_t i = 0; i < NUI_SKELETON_COUNT; ++i ) {
mSkeletons.at( i ).clear();
NUI_SKELETON_TRACKING_STATE trackingState = frameSkeleton.SkeletonData[ i ].eTrackingState;
if ( trackingState == NUI_SKELETON_TRACKED || trackingState == NUI_SKELETON_POSITION_ONLY ) {
if ( !foundSkeleton ) {
_NUI_TRANSFORM_SMOOTH_PARAMETERS transform = kTransformParams[ mTransform ];
hr = mSensor->NuiTransformSmooth( &frameSkeleton, &transform );
if ( FAILED( hr ) ) {
error( hr );
}
foundSkeleton = true;
}
if ( mFlipped ) {
( frameSkeleton.SkeletonData + i )->Position.x *= -1.0f;
for ( int32_t j = 0; j < (int32_t)NUI_SKELETON_POSITION_COUNT; ++j ) {
( frameSkeleton.SkeletonData + i )->SkeletonPositions[ j ].x *= -1.0f;
}
}
_NUI_SKELETON_BONE_ORIENTATION bones[ NUI_SKELETON_POSITION_COUNT ];
hr = NuiSkeletonCalculateBoneOrientations( frameSkeleton.SkeletonData + i, bones );
if ( FAILED( hr ) ) {
error( hr );
}
for ( int32_t j = 0; j < (int32_t)NUI_SKELETON_POSITION_COUNT; ++j ) {
Bone bone( *( ( frameSkeleton.SkeletonData + i )->SkeletonPositions + j ), *( bones + j ) );
( mSkeletons.begin() + i )->insert( std::pair<JointName, Bone>( (JointName)j, bone ) );
}
}
}
mNewSkeletons = true;
}
mFrameRate = (float)( 1.0 / ( time - mReadTime ) );
mReadTime = time;
}
}
}
return;
}
bool CalSaver::saveXmlCoreSkeleton(const std::string& strFilename, CalCoreSkeleton *pCoreSkeleton)
{
std::stringstream str;
vsxTiXmlDocument doc(strFilename);
vsxTiXmlElement skeleton("SKELETON");
//skeleton.SetAttribute("MAGIC",Cal::SKELETON_XMLFILE_MAGIC);
skeleton.SetAttribute("VERSION",Cal::LIBRARY_VERSION);
skeleton.SetAttribute("NUMBONES",pCoreSkeleton->getVectorCoreBone().size());
int boneId;
for(boneId = 0; boneId < (int)pCoreSkeleton->getVectorCoreBone().size(); ++boneId)
{
CalCoreBone* pCoreBone=pCoreSkeleton->getCoreBone(boneId);
vsxTiXmlElement bone("BONE");
bone.SetAttribute("ID",boneId);
bone.SetAttribute("NAME",pCoreBone->getName());
bone.SetAttribute("NUMCHILDS",pCoreBone->getListChildId().size());
vsxTiXmlElement translation("TRANSLATION");
const CalVector& translationVector = pCoreBone->getTranslation();
str.str("");
str << translationVector.x << " "
<< translationVector.y << " "
<< translationVector.z;
vsxTiXmlText translationdata(str.str());
translation.InsertEndChild(translationdata);
bone.InsertEndChild(translation);
vsxTiXmlElement rotation("ROTATION");
const CalQuaternion& rotationQuad = pCoreBone->getRotation();
str.str("");
str << rotationQuad.x << " "
<< rotationQuad.y << " "
<< rotationQuad.z << " "
<< rotationQuad.w;
vsxTiXmlText rotationdata(str.str());
rotation.InsertEndChild(rotationdata);
bone.InsertEndChild(rotation);
vsxTiXmlElement localtranslation("LOCALTRANSLATION");
const CalVector& localtranslationVector = pCoreBone->getTranslationBoneSpace();
str.str("");
str << localtranslationVector.x << " "
<< localtranslationVector.y << " "
<< localtranslationVector.z;
vsxTiXmlText localtranslationdata(str.str());
localtranslation.InsertEndChild(localtranslationdata);
bone.InsertEndChild(localtranslation);
vsxTiXmlElement localrotation("LOCALROTATION");
const CalQuaternion& localrotationQuad = pCoreBone->getRotationBoneSpace();
str.str("");
str << localrotationQuad.x << " "
<< localrotationQuad.y << " "
<< localrotationQuad.z << " "
<< localrotationQuad.w;
vsxTiXmlText localrotationdata(str.str());
localrotation.InsertEndChild(localrotationdata);
bone.InsertEndChild(localrotation);
vsxTiXmlElement parent("PARENTID");
str.str("");
str << pCoreBone->getParentId();
vsxTiXmlText parentid(str.str());
parent.InsertEndChild(parentid);
bone.InsertEndChild(parent);
// get children list
std::list<int>& listChildId = pCoreBone->getListChildId();
// write all children ids
std::list<int>::iterator iteratorChildId;
for(iteratorChildId = listChildId.begin(); iteratorChildId != listChildId.end(); ++iteratorChildId)
{
vsxTiXmlElement child("CHILDID");
str.str("");
//int id=*iteratorChildId;
str << *iteratorChildId;
vsxTiXmlText childid(str.str());
child.InsertEndChild(childid);
bone.InsertEndChild(child);
}
skeleton.InsertEndChild(bone);
}
doc.InsertEndChild(skeleton);
if(!doc.SaveFile())
{
CalError::setLastError(CalError::FILE_WRITING_FAILED, __FILE__, __LINE__, strFilename);
return false;
}
return true;
}
