public void UpdateTransform(VBone[] frame)
{
Vector3 pos;
Vector3 normal;
// Przygotowujemy macierze transformacji
for (int i = 0; i < frame.Length; i++)
{
if (frame[i].ParentIndex == -1)
{
KMeshHelper.CalculateBoneMatrix(i, Matrix.Identity, ref frame, ref InvBindPose, ref BoneTransform);
}
}
// Kosci
for (int i = 0; i < NumBoneVertex; i++)
{
boneVertices[i].Position = Vector3.Transform(boneBindVertices[i].Position, BoneTransform[i]);
}
vBoneBuffer.SetData(boneVertices, 0, NumBoneVertex, SetDataOptions.Discard);
// Mesh
for (int i = 0; i < NumVertex; i++)
{
pos = Vector3.Zero;
normal = Vector3.Zero;
for (int j = 0; j < 4; j++)
{
pos += Vector3.Transform(meshVertices[i].Position, Matrix.Multiply(BoneTransform[meshVertices[i].BoneIndices[j]], meshVertices[i].BoneWeight[j]));
normal += Vector3.TransformNormal(meshVertices[i].Normals, Matrix.Multiply(BoneTransform[meshVertices[i].BoneIndices[j]], meshVertices[i].BoneWeight[j]));
}
vertices[i].Position = pos;
vertices[i].Normal = normal;
}
vMeshBuffer.SetData(vertices, 0, NumVertex, SetDataOptions.Discard);
}
void DoUpdate()
{
float time = KTimer.GetElapsedTime();
// Licznik FPS
Timer_TotalTime += time;
Timer_FPSCount++;
if (Timer_TotalTime > 1.0f)
{
RenderingPanel.SetFPS(Timer_FPSCount);
Timer_TotalTime -= 1.0f;
Timer_FPSCount = 0;
}
if (model != null && AnimationManager != null)
{
if (AnimationManager.IsAnimationLoaded)
{
if (AnimationManager.IsPlaying)
{
AnimationManager.Update(time);
}
if (prevFrame != AnimationManager.CurrentFrame || AnimationManager.IsPlaying)
{
KeyFrame frame = AnimationManager.GetActualFrame();
model.UpdateTransform(frame.Bones);
BonePanel.Update(frame.Bones);
prevFrame = AnimationManager.CurrentFrame;
}
}
else
{
if (SensorManager != null)
{
if (SensorManager.IsRunning())
{
// Szkielet
VBone[] bones = KMeshHelper.CopyBones(model.GetBones());
Skeleton skel = SensorManager.GetSkeletonFrame();
if (skel != null && model != null)
{
KJointType[] JointList = BonePanel.BoneConnections.ToArray();
for (int i = 0; i < bones.Length; i++)
{
if (JointList[i] != KJointType.None)
{
JointType ktype = (JointType)JointList[i];
if (skel.Joints[ktype].TrackingState == JointTrackingState.Tracked)
{
Quaternion rotation = KKinectHelper.RecalculateKinectJointOrientation(ktype, skel);
if (bones[i].ParentIndex == -1) // Gdy kość root
{
rotation.W *= -1;
if (!RootBoneScaleSet)
{
RootBoneScale = bones[i].BonePos.Position.Z / skel.Joints[ktype].Position.Y; // KINECT.Y == XNA.Z
DistanceFromSensor = skel.Joints[ktype].Position.Z;
RootBoneScaleSet = true;
}
bones[i].BonePos.Position.X = skel.Joints[ktype].Position.X * RootBoneScale;
bones[i].BonePos.Position.Y = (skel.Joints[ktype].Position.Z - DistanceFromSensor) * RootBoneScale;
bones[i].BonePos.Position.Z = skel.Joints[ktype].Position.Y * RootBoneScale;
}
bones[i].BonePos.Orientation = rotation;
}
}
}
model.UpdateTransform(bones);
BonePanel.Update(bones);
}
if (AnimationManager.IsRecording)
{
Timer_RecordTime += time;
if (Timer_RecordTime > AnimationManager.StepTime)
{
KeyFrame key = new KeyFrame();
key.Time = AnimationManager.StepTime * (AnimationManager.GetFrameCount() + 1);
key.Bones = bones;
AnimationManager.AddKeyFrame(key);
Timer_RecordTime -= AnimationManager.StepTime;
}
}
}
}
}
}
}
public KModel(GraphicsDevice d, KVertexDeclaration[] v, short[] ind, VBone[] b)
{
device = d;
meshVertices = v;
Bones = b;
// ============= IndexBuffer =============
NumIndicies = ind.Length;
iMeshBuffer = new IndexBuffer(device, IndexElementSize.SixteenBits, sizeof(short) * (NumIndicies * 3), BufferUsage.None);
iMeshBuffer.SetData(ind);
// ============= VertexBuffer =============
NumVertex = v.Length;
vMeshBuffer = new DynamicVertexBuffer(device, typeof(VertexPositionNormalTexture), NumVertex, BufferUsage.WriteOnly);
vertices = new VertexPositionNormalTexture[NumVertex];
for (int i = 0; i < NumVertex; i++)
{
vertices[i].Position = meshVertices[i].Position;
vertices[i].Normal = meshVertices[i].Normals;
vertices[i].TextureCoordinate = meshVertices[i].TextCord;
}
vMeshBuffer.SetData(vertices);
// ============= Kosci =============
InvBindPose = new Matrix[Bones.Length];
BoneTransform = new Matrix[Bones.Length];
for (int i = 0; i < Bones.Length; i++)
{
if (Bones[i].ParentIndex == -1)
{
KMeshHelper.CalculateInvertBindPose(i, Matrix.Identity, ref Bones, ref InvBindPose);
}
}
boneBindVertices = new VertexPositionColor[Bones.Length];
boneVertices = new VertexPositionColor[Bones.Length];
for (int i = 0; i < Bones.Length; i++)
{
boneBindVertices[i].Color = Color.Gold;
boneBindVertices[i].Position = Vector3.Transform(Vector3.Zero, Matrix.Invert(InvBindPose[i]));
boneVertices[i] = boneBindVertices[i];
}
NumBoneVertex = boneVertices.Length;
vBoneBuffer = new DynamicVertexBuffer(device, typeof(VertexPositionColor), NumBoneVertex, BufferUsage.WriteOnly);
vBoneBuffer.SetData(boneVertices);
short[] bind = new short[Bones.Length * 2];
short p = 0;
for (int i = 0; i < Bones.Length * 2; i += 2)
{
bind[i + 0] = p;
if (Bones[p].ParentIndex > -1)
{
bind[i + 1] = (short)Bones[p].ParentIndex;
}
else
{
bind[i + 1] = p;
}
p++;
}
NumBoneIndicies = bind.Length;
iBoneBuffer = new IndexBuffer(device, IndexElementSize.SixteenBits, sizeof(short) * (NumBoneIndicies), BufferUsage.None);
iBoneBuffer.SetData(bind);
Matrix Projection = Matrix.CreatePerspectiveFieldOfView(MathHelper.ToRadians(45), device.Viewport.AspectRatio, 0.01f, 1000f);
// ============= Shader'y =============
MeshShader = new BasicEffect(device);
MeshShader.EnableDefaultLighting();
MeshShader.World = Matrix.Identity;
MeshShader.Projection = Projection;
BoneShader = new BasicEffect(device);
BoneShader.VertexColorEnabled = true;
BoneShader.World = Matrix.Identity;
BoneShader.Projection = Projection;
DrawBones = true;
DrawMesh = true;
}
