public Grammar()
{
Options = RuntimeOptions.Default;
Productions = new ProductionCollection(this);
Symbols = new SymbolCollection(this);
Conditions = new ConditionCollection(this);
Matchers = new MatcherCollection(this);
Mergers = new MergerCollection(this);
Contexts = new ForeignContextCollection(this);
Reports = new ReportCollection();
GlobalContextProvider = new ForeignContextProvider { Owner = this.Contexts };
Joint = new Joint();
for (int i = PredefinedTokens.Count; i != 0; --i)
{
Symbols.Add(null); // stub
}
Symbols[PredefinedTokens.Propagated] = new Symbol("#");
Symbols[PredefinedTokens.Epsilon] = new Symbol("$eps");
Symbols[PredefinedTokens.AugmentedStart] = new Symbol("$start");
Symbols[PredefinedTokens.Eoi] = new Symbol("$")
{
Categories = SymbolCategory.DoNotInsert
| SymbolCategory.DoNotDelete
};
Symbols[PredefinedTokens.Error] = new Symbol("$error");
AugmentedProduction = Productions.Define((Symbol)Symbols[PredefinedTokens.AugmentedStart], new Symbol[] { null });
}
private void SetUIElementPosition(FrameworkElement element, Joint joint, int yOffset)
{
var scaledJoint = joint.ScaleTo(1024, 768, .99f, .99f);
Canvas.SetLeft(element, scaledJoint.Position.X);
Canvas.SetTop(element, scaledJoint.Position.Y - yOffset);
}
public Gesture(DateTime timestamp, double magnitude, GestureID id, Joint guestureSource)
{
this._timestamp = timestamp;
this._magnitude = magnitude;
this._id = id;
this._guestureSource = guestureSource;
}
public Direction(Joint i, Joint j)
{
createDirection(
j.Position.X - i.Position.X,
j.Position.Y - i.Position.Y,
j.Position.Z - i.Position.Z);
}
public void release()
{
Joint oldGrip = grip;
grip = null;
Object.Destroy(oldGrip);
grabee = null;
}
void SetGrapple(GameObject targ, int grappleType)
{
if (targ == null) return;
if (curTarget != null) FreeGrapple ();
swinging = true;
curTarget = targ;
Rigidbody targRigid = curTarget.GetComponent<Rigidbody> ();
if (targRigid != null) {
if (grappleType == 0) {
HingeJoint target_hinge = targRigid.gameObject.AddComponent <HingeJoint> ();
target_hinge.connectedBody = rigid;
// parentRigid.isKinematic = true;
target_hinge.autoConfigureConnectedAnchor = false;
target_hinge.connectedAnchor = targRigid.transform.position;
hingeTarget = target_hinge;
// target_hinge.maxDistance = 50;
// target_hinge.minDistance = 45;
} else if (grappleType == 1) {
SpringJoint target_spring = targRigid.gameObject.AddComponent <SpringJoint> ();
target_spring.connectedBody = rigid;
target_spring.autoConfigureConnectedAnchor = false;
target_spring.connectedAnchor = targRigid.transform.position;
target_spring.maxDistance = 50;
target_spring.minDistance = 5;
hingeTarget = target_spring;
}
}
}
/// <summary>
/// Process walking/running. Calculates how far forward the right foot
/// is from the left foot.
/// </summary>
/// <param name="rightFoot"></param>
/// <param name="leftFoot"></param>
private void detectWalking(Joint rightFoot, Joint leftFoot) {
double feetDifferential = leftFoot.Position.Z - rightFoot.Position.Z;
// Move forward
if (feetDifferential > walkThresh)
{
if (feetDifferential > runThresh)
{
InputSimulator.SimulateKeyDown(VirtualKeyCode.VK_2);
}
else
{
InputSimulator.SimulateKeyDown(VirtualKeyCode.VK_W);
}
}
// Move backward
else if (feetDifferential < -walkThresh)
{
InputSimulator.SimulateKeyDown(VirtualKeyCode.VK_S);
}
else
{
if (InputSimulator.IsKeyDown(VirtualKeyCode.VK_W)) InputSimulator.SimulateKeyUp(VirtualKeyCode.VK_W);
if (InputSimulator.IsKeyDown(VirtualKeyCode.VK_S)) InputSimulator.SimulateKeyUp(VirtualKeyCode.VK_S);
if (InputSimulator.IsKeyDown(VirtualKeyCode.VK_2)) InputSimulator.SimulateKeyUp(VirtualKeyCode.VK_2);
}
}
public KinectSkeleton(Joint ankleLeft, Joint ankleRight, Joint elbowLeft, Joint elbowRight, Joint footLeft,
Joint footRight, Joint handLeft, Joint handRight, Joint head, Joint hipCenter,
Joint hipLeft, Joint hipRight, Joint kneeLeft, Joint kneeRight, Joint shoulderCenter,
Joint shoulderLeft, Joint shoulderRight, Joint spine, Joint wristLeft, Joint wristRight)
{
this.ankleLeft = ankleLeft;
this.ankleRight = ankleRight;
this.elbowLeft = elbowLeft;
this.elbowRight = elbowRight;
this.footLeft = footLeft;
this.footRight = footRight;
this.handLeft = handLeft;
this.handRight = handRight;
this.head = head;
this.hipCenter = hipCenter;
this.hipLeft = hipLeft;
this.hipRight = hipRight;
this.kneeLeft = kneeLeft;
this.kneeRight = kneeRight;
this.shoulderCenter = shoulderCenter;
this.shoulderLeft = shoulderLeft;
this.shoulderRight = shoulderRight;
this.spine = spine;
this.wristLeft = wristLeft;
this.wristRight = wristRight;
}
void OnCollisionEnter(Collision col)
{
if (col.gameObject.name == "BrickNew (5)")
{
theJoint = gameObject.AddComponent<FixedJoint>();
theJoint.connectedBody = col.rigidbody;
}
}
// OnCollisionEnter is called when this
// collider/rigidbody has begun touching
// another rigidbody/collider.
void OnCollisionEnter(Collision collision)
{
Linkable linkee = collision.gameObject.GetComponent<Linkable>();
if (linkee != null) {
joint = this.gameObject.AddComponent<FixedJoint>();
joint.connectedBody = linkee.gameObject.rigidbody;
}
}
public void NullCanBeAddedAndHasNoEffect()
{
var target = new Joint();
target.Add(null);
Assert.IsFalse(target.Has<object>());
Assert.IsNull(target.Get<object>());
Assert.Throws<InvalidOperationException>(() => target.The<object>());
}
public static Vector3D vectorFromJoint(Joint j)
{
Vector3D v = new Vector3D();
v.X = j.Position.X;
v.Y = j.Position.Y;
v.Z = j.Position.Z;
return v;
}
public new void Awake() {
base.Awake();
_path = transform.Find("Path").GetComponent<BoxCollider>();
_cursor = transform.Find("Path/Cursor").GetComponent<Joint>();
_slothSnapper = GameObject.FindGameObjectWithTag("SlothNinja").GetComponent<Snapper>();
}
/*
Constructor: Plane
Initializes x,y,z.
*/
public Plane(Joint a, Joint b, Joint c)
{
Direction d1 = new Direction(a, b);
Direction d2 = new Direction(a, c);
Direction per = Mathematics.crossProduct(d1, d2);
x = per.x;
y = per.y;
z = per.z;
}
public static List<GameObject> NetworkLink(Vector3 currentPosition, Vector3 finalPosition, Joint hook, Rigidbody anchor, bool gravity)
{
List<GameObject> links = new List<GameObject>();
GameObject link = ResourceFactory.GetInstance().GetPrefab(Registry.Prefab.ChainLink);
float linkHeight = link.GetComponent<Renderer>().bounds.max.y * 2;
// Get any vector perpindicular to the direction towards the bird in order to get the Quaternion
Vector3 direction = finalPosition - currentPosition;
Vector3 forward = Vector3.RotateTowards(direction, -direction, Mathf.PI/2f, 0);
Quaternion linkRotation = Quaternion.LookRotation(forward, direction);
GameObject currentLink = null;
ConfigurableJoint prevBody = null;
while(Vector3.Distance(currentPosition, finalPosition) > linkHeight)
{
if(uLink.Network.isServer)
{
currentLink = uLink.Network.Instantiate(uLink.Network.player, Registry.Prefab.ProxyChainLink, Registry.Prefab.ServerChainLink,
Registry.Prefab.ServerChainLink, currentPosition, linkRotation, 0);
}
else
{
currentLink = uLink.Network.Instantiate(uLink.Network.player, Registry.Prefab.ProxyChainLink, Registry.Prefab.ServerChainLink,
Registry.Prefab.ProxyChainLink, currentPosition, linkRotation, 0);
}
currentLink.transform.parent = hook.transform;
currentPosition += currentLink.transform.up * linkHeight;
links.Add(currentLink);
Rigidbody currentBody = currentLink.GetComponent<Rigidbody>();
currentBody.useGravity = gravity;
if(hook.connectedBody == null)
{
hook.connectedBody = currentBody;
}
else
{
ConnectJoint(prevBody, currentBody);
}
prevBody = currentLink.GetComponent<ConfigurableJoint>();
}
if(anchor != null)
{
ConnectJoint(currentLink.GetComponent<ConfigurableJoint>(), anchor);
}
else
{
currentLink.GetComponent<ConfigurableJoint>().connectedAnchor = finalPosition;
}
currentLink.GetComponent<ConfigurableJoint>().autoConfigureConnectedAnchor = true;
return links;
}
/// <summary>
/// Grip a rigidbody with a hand
/// </summary>
/// <param name="target">Target rigidbody</param>
/// <param name="hand">Which hand to grip with, "left" or "right"</param>
public void Grip(Joint target, string hand) {
_ragdoller.ragdolled = true;
IsGripped = true;
if (hand == "left") {
target.connectedBody = this._leftHand;
} else {
target.connectedBody = this._rightHand;
}
}
public void SayGoodbye(Joint joint)
{
if (test._mouseJoint == joint)
{
test._mouseJoint = null;
}
else
{
test.JointDestroyed(joint);
}
}
public void Flip(Joint activeHand)
{
double scaleX = activeHand.JointType == JointType.HandRight ? 1.0 : -1.0;
ScaleTransform transform = root.RenderTransform as ScaleTransform;
if (transform.ScaleX != scaleX)
{
transform.ScaleX = scaleX;
}
}
/**********************************la fonction du skeletons*************************************************************/
private Point getDisplayPosition(Joint joint)
{
float depthX, depthY;
_nui.SkeletonEngine.SkeletonToDepthImage(joint.Position, out depthX, out depthY);
depthX = Math.Max(0, Math.Min(depthX * 320, 320));
depthY = Math.Max(0, Math.Min(depthY * 240, 240));
int colorX, colorY;
ImageViewArea iv = new ImageViewArea();
_nui.NuiCamera.GetColorPixelCoordinatesFromDepthPixel(ImageResolution.Resolution640x480, iv, (int)depthX, (int)depthY, (short)0, out colorX, out colorY);
return new Point((int)(skeleton.Width * colorX / 640.0), (int)(skeleton.Height * colorY / 480));
}
private void compute(Joint shoulder, Joint elbow, Joint wrist)
{
Vector3D shoulderVec = Util.vectorFromJoint(shoulder);
Vector3D elbowVec = Util.vectorFromJoint(elbow);
Vector3D wristVec = Util.vectorFromJoint(wrist);
Vector3D elbowToShoulder = shoulderVec - elbowVec;
Vector3D elbowToWrist = wristVec - elbowVec;
Yaw = Util.degToRad(Vector3D.AngleBetween(elbowToWrist, elbowToShoulder));
}
public override void Awake()
{
base.Awake();
if(mJoint == null)
{
mJoint = GetComponent<Joint>();
}
if(mRenderer == null)
{
mRenderer = GetComponentInChildren<Renderer>();
}
}
public void GetJoints()
{
var bodyReader = BodyFrameTest.GetBodyFrameReader();
using ( var bodyFrame = bodyReader.AcquireLatestFrame() ) {
Body[] bodies = new Body[6];
bodyFrame.GetAndRefreshBodyData( bodies );
foreach ( var body in bodies ) {
Joint[] joints = new Joint[(int)JointType.Count];
body.GetJoints( joints );
}
}
}
private void getCoordinates_Playsound(Joint leftFoot)
{
//Decimal.Round((decimal)(leftFoot.Position.X),2) is to round the Decimal number to the nearest 2 decimals
leftTextBlock.Text = "X= " + Decimal.Round((decimal)(leftFoot.Position.X), 2) + " , Y= " + Decimal.Round((decimal)(leftFoot.Position.Y), 2) + " , Z= " + Decimal.Round((decimal)(leftFoot.Position.Z), 2);
if (leftFoot.Position.Y < 0.2f)
{
// left hand x pos varies from -1>>2 we add +2 to make it 1>>2 and freq from 600>>1200
Console.Beep((int)((leftFoot.Position.X+2)/1*600),500);
}
}
/// <summary>
/// Constructora de copia que inicializa el LineElement como un clon de otro.
/// </summary>
/// <param name="src">El LineElement a copiar</param>
internal LineElement(LineElement src, Joint i, Joint j) : base(src)
{
props = (LineProps)src.Properties.Clone();
joints = new Joint[2];
angle = src.Angle;
cardinalPoint = src.CardinalPoint;
offsets = src.EndOffsets;
dofI = src.DoFI;
dofJ = src.DoFJ;
joints[0] = i;
joints[1] = j;
}
private void kinectSensor_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
foreach (SkeletonData s in e.SkeletonFrame.Skeletons)
{
if (s.TrackingState == SkeletonTrackingState.Tracked)
{
resolution = new Vector2(640, 480);
skeleton = s;
rightHandJoint = skeleton.Joints[JointID.HandRight];
position = new Vector2((((0.5f * rightHandJoint.Position.X) + 0.5f) * (resolution.X)), (((-0.5f * rightHandJoint.Position.Y) + 0.5f) * (resolution.Y)));
}
}
}
private Point getVideoPoint( Joint joint )
{
float depthX = 0, depthY = 0;
runtime.SkeletonEngine.SkeletonToDepthImage( joint.Position, out depthX, out depthY );
depthX = Math.Min( depthX * runtime.DepthStream.Width, runtime.DepthStream.Width );
depthY = Math.Min( depthY * runtime.DepthStream.Height, runtime.DepthStream.Height );
int videoX = 0, videoY = 0;
runtime.NuiCamera.GetColorPixelCoordinatesFromDepthPixel( ImageResolution.Resolution640x480,
new ImageViewArea(), (int)depthX, (int)depthY, 0, out videoX, out videoY );
return new Point( Math.Min( videoX, runtime.VideoStream.Width ), Math.Min( videoY, runtime.VideoStream.Height ) );
}
public static bool CanFindPath(Joint J1, Joint J2, int sx, int sy, int ex, int ey)
{
ComponentsManager.IgnoreAS1 = J1;
ComponentsManager.IgnoreAS2 = J2;
var path = RunPathFinder(sx, sy, ex, ey);
if (path == null) return false;
ComponentsManager.IgnoreAS1 = null;
ComponentsManager.IgnoreAS2 = null;
return true;
}
public void use(ToolUseData d)
{
if(grabee != null) {
release();
return;
}
RaycastHit info;
Ray checkRay = new Ray(d.position, d.heading);
if(Physics.Raycast(checkRay, out info, grabRange)) {
grabee = info.collider.gameObject;
grip = this.gameObject.AddComponent<FixedJoint>();
grip.connectedBody = grabee.gameObject.rigidbody;
}
}
private Point getDisplayPosition(Joint joint)
{
float depthX, depthY;
myRuntime.SkeletonEngine.SkeletonToDepthImage(joint.Position, out depthX, out depthY);
depthX = depthX * 320; //convert to 320, 240 space
depthY = depthY * 240; //convert to 320, 240 space
int colorX, colorY;
ImageViewArea iv = new ImageViewArea();
// only ImageResolution.Resolution640x480 is supported at this point
myRuntime.NuiCamera.GetColorPixelCoordinatesFromDepthPixel(ImageResolution.Resolution640x480, iv, (int)depthX, (int)depthY, (short)0, out colorX, out colorY);
// map back to skeleton.Width & skeleton.Height
return new Point((int)(preview.Width * colorX / 640.0), (int)(preview.Height * colorY / 480));
}
public void LoadJointProfiles()
{
// RBakerFlag -> TEST CODE!
PinMapping cw = new PinMapping(9, "clockwise");
PinMapping ccw = new PinMapping(10, "anti-clockwise");
arduinoSerialPort.ComponentMappings.Add(cw);
arduinoSerialPort.ComponentMappings.Add(ccw);
cw.SetPinMode(SetPinModeStateCodes.OutputStateCode);
ccw.SetPinMode(SetPinModeStateCodes.OutputStateCode);
Joint joint = new Joint(cw, ccw, JointType.ElbowRight);
joints.Add(joint);
}
private static void DumpTree(Transform t, DumpTreeOption options, int level, StringBuilder sb)
{
string space = "";
for (int i = 0; i < level; ++i)
{
space += '-';
}
sb.AppendLine(space + t.name);
if ((options & DumpTreeOption.Active) == DumpTreeOption.Active)
{
sb.AppendLine(space + "+ activeSelf=" + t.gameObject.activeSelf + " activeInHeirachy=" + t.gameObject.activeInHierarchy);
}
if ((options & DumpTreeOption.TransformPosition) == DumpTreeOption.TransformPosition)
{
sb.AppendLine(space + "+ position: " + t.position + " localPosition: " + t.localPosition);
}
if ((options & DumpTreeOption.TransformRotation) == DumpTreeOption.TransformRotation)
{
sb.AppendLine(space + "+ rotation: " + t.rotation + " localRotation: " + t.localRotation);
}
if ((((int)options & 0x10) == 0))
{
goto skipComponents;
}
foreach (Component c in t.gameObject.GetComponents <Component>())
{
if (!(c is Transform) && ((options & DumpTreeOption.Components) == DumpTreeOption.Components))
{
sb.AppendLine(space + "+ component:" + c.GetType());
}
if (c is Renderer && (options & DumpTreeOption.Materials) == DumpTreeOption.Materials)
{
foreach (Material m in t.renderer.sharedMaterials)
{
sb.AppendLine(space + "++ mat:" + m.name);
}
}
if (c is MeshFilter && (options & DumpTreeOption.Mesh) == DumpTreeOption.Mesh)
{
MeshFilter filter = (MeshFilter)c;
sb.AppendLine(space + "++ mesh:" + ((filter.sharedMesh == null) ? "*null*" : (filter.sharedMesh.name + " verts:" + filter.sharedMesh.vertexCount)));
}
if (c is MeshCollider && (options & DumpTreeOption.Mesh) == DumpTreeOption.Mesh)
{
MeshCollider collider = (MeshCollider)c;
sb.AppendLine(space + "++ mesh:" + ((collider.sharedMesh == null) ? "*null*" : (collider.sharedMesh.name + " verts:" + collider.sharedMesh.vertexCount)));
}
if (c is Rigidbody && (options & DumpTreeOption.Rigidbody) == DumpTreeOption.Rigidbody)
{
Rigidbody body = (Rigidbody)c;
sb.AppendLine(space + "++ Mass:" + body.mass.ToString("F3"));
sb.AppendLine(space + "++ CoM:" + body.centerOfMass.ToString("F3"));
}
if (c is Joint && (options & DumpTreeOption.Rigidbody) == DumpTreeOption.Rigidbody)
{
Joint joint = (Joint)c;
sb.AppendLine(space + "++ anchor:" + joint.anchor.ToString("F3"));
sb.AppendLine(space + "++ connectedBody: " + (joint.connectedBody != null));
if (joint.connectedBody != null)
{
DumpTree(joint.connectedBody.transform, options, level + 1, sb);
}
}
}
skipComponents:
for (int i = 0; i < t.childCount; ++i)
{
DumpTree(t.GetChild(i), options, level + 1, sb);
}
}
void readMesh(ref Model model, BinaryReader reader)
{
var meshCount = reader.ReadUInt16();
model.meshes = new List <Mesh>();
for (var i = 0; i < meshCount; i++)
{
Mesh newMesh = new Mesh();
newMesh.meshType = (MeshType)reader.ReadByte();
if (newMesh.meshType == MeshType.Standard)
{
newMesh.visualMeshType = (VisualMeshType)reader.ReadByte();
newMesh.meshRenderFlags = (MeshRenderFlag)reader.ReadUInt16();
}
newMesh.parentID = reader.ReadUInt16();
newMesh.pos = ReadVector3(reader);
newMesh.scale = ReadVector3(reader);
newMesh.rot = ReadQuat(reader);
var rot = newMesh.rot;
var tmpRot = new Quaternion(rot.y, rot.z, rot.w, -1 * rot.x);
newMesh.rot = tmpRot;
newMesh.cullingFlags = (MeshOccludingFlag)reader.ReadByte();
newMesh.meshName = ReadString(reader);
newMesh.meshParams = ReadString(reader);
switch (newMesh.meshType)
{
case MeshType.Standard:
{
switch (newMesh.visualMeshType)
{
case VisualMeshType.Standard:
{
newMesh.standard = readStandard(reader);
}
break;
case VisualMeshType.Mirror:
{
newMesh.mirror = readMirror(reader);
}
break;
case VisualMeshType.Glow:
{
newMesh.glow = readGlow(reader);
}
break;
case VisualMeshType.Billboard:
{
Billboard newBillboard = new Billboard();
newBillboard.standard = readStandard(reader);
newBillboard.rotationAxis = reader.ReadUInt32();
newBillboard.ignoreCamera = reader.ReadByte() > 0;
newMesh.billboard = newBillboard;
}
break;
case VisualMeshType.Morph:
{
newMesh.morph = readMorph(reader, false);
}
break;
case VisualMeshType.Single_Mesh:
{
newMesh.singleMesh = readSingleMesh(reader);
}
break;
case VisualMeshType.Single_Morph:
{
newMesh.singleMorph = readSingleMorph(reader);
}
break;
default:
{
}
break;
}
}
break;
case MeshType.Dummy:
{
Dummy newDummy = new Dummy();
newDummy.minBox = ReadVector3(reader);
newDummy.maxBox = ReadVector3(reader);
newMesh.dummy = newDummy;
}
break;
case MeshType.Sector:
{
newMesh.sector = readSector(reader);
}
break;
case MeshType.Target:
{
newMesh.target = readTarget(reader);
}
break;
case MeshType.Joint:
{
Joint newJoint = new Joint();
newJoint.transform = ReadMatrix(reader);
newJoint.boneID = reader.ReadUInt32();
newMesh.joint = newJoint;
}
break;
default:
{
}
break;
}
// NOTE(zaklaus): Check whether this is a collision mesh.
// happens AFTER we load the required content to skip it.
string meshName = newMesh.meshName;
if (meshName.Contains("wcol"))
{
newMesh.meshType = MeshType.Collision;
}
model.meshes.Add(newMesh);
}
}
public static Skeleton NormalizeDataUseShoulder(Skeleton data)
{
if (data.TrackingState == Microsoft.Kinect.SkeletonTrackingState.Tracked)
{
// // Extract the coordinates of the points.
SkeletonPoint center = getCenterPoint(data.Joints[JointType.ShoulderLeft], data.Joints[JointType.ShoulderRight]);
SkeletonPoint slcenter = data.Joints[JointType.ShoulderLeft].Position;
// float shoulderDist = (float) getDistance(data.Joints[JointType.ShoulderLeft], data.Joints[JointType.ShoulderRight]);
float handLength = (float)(getDistance(data.Joints[JointType.HandLeft], data.Joints[JointType.WristLeft])
+ getDistance(data.Joints[JointType.WristLeft], data.Joints[JointType.ElbowLeft])
+ getDistance(data.Joints[JointType.ElbowLeft], data.Joints[JointType.ShoulderLeft]));
// //change origin
foreach (Joint j in data.Joints)
{
SkeletonPoint normalizedOriginPoint = new SkeletonPoint();
// normalizedOriginPoint.X = j.Position.X - slcenter.X;
// normalizedOriginPoint.Y = j.Position.Y - slcenter.Y;
normalizedOriginPoint.X = j.Position.X;
normalizedOriginPoint.Y = j.Position.Y;
// // Use only hand position in font of the user
normalizedOriginPoint.Z = (j.Position.Z - slcenter.Z) + handLength / 2;// +0.1f;
Joint joint = data.Joints[j.JointType];
joint.Position = normalizedOriginPoint;
data.Joints[j.JointType] = joint;
}
// // Normalization of the coordinates
//foreach (Joint j in data.Joints)
//{
// SkeletonPoint normalizedDistancePoint = new SkeletonPoint();
// //normalizedDistancePoint.X = j.Position.X / (handLength);
// //normalizedDistancePoint.Y = j.Position.Y / handLength;
// normalizedDistancePoint.X = j.Position.X;
// normalizedDistancePoint.Y = j.Position.Y;
// normalizedDistancePoint.Z = j.Position.Z / (handLength / -2);
// Joint joint = data.Joints[j.JointType];
// joint.Position = normalizedDistancePoint;
// data.Joints[j.JointType] = joint;
//}
// var left = data.Joints[JointType.HandLeft];
// var right = data.Joints[JointType.HandRight];
// //var middle = data.Joints[JointType.];
// //KinectBlenderServer.PublishData(left.Position.X + "," + left.Position.Y + "," + left.Position.Z + "," +
// // right.Position.X + "," + right.Position.Y + "," + right.Position.Z);
// KinectBlenderServer.PublishData(left.Position.X + "," + left.Position.Y + "," + left.Position.Z + "," +
// right.Position.X + "," + right.Position.Y + "," + right.Position.Z);
// // Launch the event!
SkeletonNormalizedDataReady(null, new SkeletonNormalizedDataEventArgs(data));
}
return(data);
}
Joint UpdateJoint(Body body, JointType jt, TRANSFORM_SMOOTH_PARAMETERS smoothingParams)
{
CameraSpacePoint vPrevRawPosition;
CameraSpacePoint vPrevFilteredPosition;
CameraSpacePoint vPrevTrend;
CameraSpacePoint vRawPosition;
CameraSpacePoint vFilteredPosition;
CameraSpacePoint vPredictedPosition;
CameraSpacePoint vDiff;
CameraSpacePoint vTrend;
float fDiff;
bool bJointIsValid;
Joint joint = body.Joints[jt];
vRawPosition = joint.Position;
vPrevFilteredPosition = m_pHistory[(int)jt].m_vFilteredPosition;
vPrevTrend = m_pHistory[(int)jt].m_vTrend;
vPrevRawPosition = m_pHistory[(int)jt].m_vRawPosition;
bJointIsValid = JointPositionIsValid(vRawPosition);
// If joint is invalid, reset the filter
if (!bJointIsValid)
{
m_pHistory[(int)jt].m_dwFrameCount = 0;
}
// Initial start values
if (m_pHistory[(int)jt].m_dwFrameCount == 0)
{
vFilteredPosition = vRawPosition;
vTrend = CSVectorZero();
m_pHistory[(int)jt].m_dwFrameCount++;
}
else if (m_pHistory[(int)jt].m_dwFrameCount == 1)
{
vFilteredPosition = CSVectorScale(CSVectorAdd(vRawPosition, vPrevRawPosition), 0.5f);
vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
m_pHistory[(int)jt].m_dwFrameCount++;
}
else
{
// First apply jitter filter
vDiff = CSVectorSubtract(vRawPosition, vPrevFilteredPosition);
fDiff = CSVectorLength(vDiff);
if (fDiff <= smoothingParams.fJitterRadius)
{
vFilteredPosition = CSVectorAdd(CSVectorScale(vRawPosition, fDiff / smoothingParams.fJitterRadius),
CSVectorScale(vPrevFilteredPosition, 1.0f - fDiff / smoothingParams.fJitterRadius));
}
else
{
vFilteredPosition = vRawPosition;
}
// Now the double exponential smoothing filter
vFilteredPosition = CSVectorAdd(CSVectorScale(vFilteredPosition, 1.0f - smoothingParams.fSmoothing),
CSVectorScale(CSVectorAdd(vPrevFilteredPosition, vPrevTrend), smoothingParams.fSmoothing));
vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
}
// Predict into the future to reduce latency
vPredictedPosition = CSVectorAdd(vFilteredPosition, CSVectorScale(vTrend, smoothingParams.fPrediction));
// Check that we are not too far away from raw data
vDiff = CSVectorSubtract(vPredictedPosition, vRawPosition);
fDiff = CSVectorLength(vDiff);
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
vPredictedPosition = CSVectorAdd(CSVectorScale(vPredictedPosition, smoothingParams.fMaxDeviationRadius / fDiff),
CSVectorScale(vRawPosition, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
}
// Save the data from this frame
m_pHistory[(int)jt].m_vRawPosition = vRawPosition;
m_pHistory[(int)jt].m_vFilteredPosition = vFilteredPosition;
m_pHistory[(int)jt].m_vTrend = vTrend;
// Output the data
m_pFilteredJoints[(int)jt] = vPredictedPosition;
joint.Position = vPredictedPosition;
return(joint);
}
//send faces found in a frame
private void SendFaces(ColorFrame colorFrame)
{
FrameDescription colorFrameDescription = _kinect.ColorFrameSource.FrameDescription;
uint bytesPerpixel = 4;//colorFrameDescription.BytesPerPixel;
byte[] allBytes = new byte[(uint)(colorFrameDescription.Width * colorFrameDescription.Height * bytesPerpixel)];
colorFrame.CopyConvertedFrameDataToArray(allBytes, ColorImageFormat.Bgra);
Mat frame = new Mat(colorFrameDescription.Height, colorFrameDescription.Width, Emgu.CV.CvEnum.DepthType.Cv8U, (int)bytesPerpixel);
colorFrame.CopyConvertedFrameDataToIntPtr(frame.DataPointer, (uint)(colorFrameDescription.Width * colorFrameDescription.Height * bytesPerpixel), ColorImageFormat.Bgra);
List <VideoFrame.Face> faces = new List <VideoFrame.Face>();
bool faceFound = false;
for (int f = 0; f < 6; f++)
{
if (_faceFrameResults[f] != null)
{
faceFound = true;
FaceFrameResult faceResult = _faceFrameResults[f];
//Return -1 if maybe
int isSpeaking = -1;
if (faceResult.FaceProperties[FaceProperty.MouthOpen] == DetectionResult.Yes ||
faceResult.FaceProperties[FaceProperty.MouthMoved] == DetectionResult.Yes)
{
isSpeaking = 1;
}
else if (faceResult.FaceProperties[FaceProperty.MouthOpen] == DetectionResult.No ||
faceResult.FaceProperties[FaceProperty.MouthMoved] == DetectionResult.No)
{
isSpeaking = 0;
}
RectI boundingBox = faceResult.FaceBoundingBoxInColorSpace;
int xCoord = boundingBox.Left;
int yCoord = boundingBox.Top;
int width = boundingBox.Right - boundingBox.Left;
int height = boundingBox.Bottom - boundingBox.Top;
Joint head = _bodies[f].Joints[JointType.Head];
var face = new VideoFrame.Face(
new Rectangle(xCoord, yCoord, width, height),
(long)_bodies[f].TrackingId,
head.Position.Z,
isSpeaking
);
faces.Add(face);
}
}
if (faceFound)
{
_logger.LogTrace("Kinect: Got frame. Found {0} faces.", faces.Count);
FrameReady?.Invoke(this, new VideoFrame(
DateTime.Now,
faces.ToList(),
frame.ToImage <Bgr, byte>(),
colorFrame.FrameDescription.Width,
colorFrame.FrameDescription.Height
));
}
}
public void Solve(ref TimeStep step, ref Vector2 gravity)
{
// Integrate velocities and apply damping.
for (int i = 0; i < this.BodyCount; ++i)
{
Body b = this.Bodies[i];
if (b.BodyType != BodyType.Dynamic)
{
continue;
}
// Integrate velocities.
// FPE 3 only - Only apply gravity if the body wants it.
if (b.IgnoreGravity)
{
b.LinearVelocityInternal.X += step.dt * (b.InvMass * b.Force.X);
b.LinearVelocityInternal.Y += step.dt * (b.InvMass * b.Force.Y);
b.AngularVelocityInternal += step.dt * b.InvI * b.Torque;
}
else
{
b.LinearVelocityInternal.X += step.dt * (gravity.X + b.InvMass * b.Force.X);
b.LinearVelocityInternal.Y += step.dt * (gravity.Y + b.InvMass * b.Force.Y);
b.AngularVelocityInternal += step.dt * b.InvI * b.Torque;
}
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f);
b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.AngularDamping, 0.0f, 1.0f);
}
// Partition contacts so that contacts with static bodies are solved last.
int i1 = -1;
for (int i2 = 0; i2 < this.ContactCount; ++i2)
{
Fixture fixtureA = this._contacts[i2].FixtureA;
Fixture fixtureB = this._contacts[i2].FixtureB;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static;
if (nonStatic)
{
++i1;
//TODO: Only swap if they are not the same? see http://code.google.com/p/box2d/issues/detail?id=162
Contact tmp = this._contacts[i1];
this._contacts[i1] = this._contacts[i2];
this._contacts[i2] = tmp;
}
}
// Initialize velocity constraints.
this._contactSolver.Reset(this._contacts, this.ContactCount, step.dtRatio, Settings.EnableWarmstarting);
this._contactSolver.InitializeVelocityConstraints();
#pragma warning disable 0162 // Unreachable code detected
if (Settings.EnableWarmstarting)
{
this._contactSolver.WarmStart();
}
#pragma warning restore 0162 // Unreachable code detected
if (Settings.EnableDiagnostics)
{
this._watch.Start();
this._tmpTime = 0;
}
for (int i = 0; i < this.JointCount; ++i)
{
if (this._joints[i].Enabled)
{
this._joints[i].InitVelocityConstraints(ref step);
}
}
if (Settings.EnableDiagnostics)
{
this._tmpTime += this._watch.ElapsedTicks;
}
// Solve velocity constraints.
for (int i = 0; i < Settings.VelocityIterations; ++i)
{
if (Settings.EnableDiagnostics)
{
this._watch.Start();
}
for (int j = 0; j < this.JointCount; ++j)
{
Joint joint = this._joints[j];
if (!joint.Enabled)
{
continue;
}
joint.SolveVelocityConstraints(ref step);
joint.Validate(step.inv_dt);
}
if (Settings.EnableDiagnostics)
{
this._watch.Stop();
this._tmpTime += this._watch.ElapsedTicks;
this._watch.Reset();
}
this._contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
this._contactSolver.StoreImpulses();
// Integrate positions.
for (int i = 0; i < this.BodyCount; ++i)
{
Body b = this.Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
// Check for large velocities.
float translationX = step.dt * b.LinearVelocityInternal.X;
float translationY = step.dt * b.LinearVelocityInternal.Y;
float result = translationX * translationX + translationY * translationY;
if (result > Settings.MaxTranslationSquared)
{
float sq = (float)Math.Sqrt(result);
float ratio = Settings.MaxTranslation / sq;
b.LinearVelocityInternal.X *= ratio;
b.LinearVelocityInternal.Y *= ratio;
}
float rotation = step.dt * b.AngularVelocityInternal;
if (rotation * rotation > Settings.MaxRotationSquared)
{
float ratio = Settings.MaxRotation / Math.Abs(rotation);
b.AngularVelocityInternal *= ratio;
}
// Store positions for continuous collision.
b.Sweep.C0.X = b.Sweep.C.X;
b.Sweep.C0.Y = b.Sweep.C.Y;
b.Sweep.A0 = b.Sweep.A;
// Integrate
b.Sweep.C.X += step.dt * b.LinearVelocityInternal.X;
b.Sweep.C.Y += step.dt * b.LinearVelocityInternal.Y;
b.Sweep.A += step.dt * b.AngularVelocityInternal;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < Settings.PositionIterations; ++i)
{
bool contactsOkay = this._contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
if (Settings.EnableDiagnostics)
{
this._watch.Start();
}
for (int j = 0; j < this.JointCount; ++j)
{
Joint joint = this._joints[j];
if (!joint.Enabled)
{
continue;
}
bool jointOkay = joint.SolvePositionConstraints();
jointsOkay = jointsOkay && jointOkay;
}
if (Settings.EnableDiagnostics)
{
this._watch.Stop();
this._tmpTime += this._watch.ElapsedTicks;
this._watch.Reset();
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
if (Settings.EnableDiagnostics)
{
this.JointUpdateTime = this._tmpTime;
}
Report(this._contactSolver.Constraints);
if (Settings.AllowSleep)
{
float minSleepTime = Settings.MaxFloat;
for (int i = 0; i < this.BodyCount; ++i)
{
Body b = this.Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0 ||
b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr ||
Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += step.dt;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= Settings.TimeToSleep)
{
for (int i = 0; i < this.BodyCount; ++i)
{
Body b = this.Bodies[i];
b.Awake = false;
}
}
}
}
public JointTypePair(Joint a, Joint b)
{
A = a.JointType;
B = b.JointType;
}
public RealPosition(Joint a)
{
this.realZ = a.Position.Z;
this.realX = System.Math.Tan(Math.PI * 28.5 / 180) * a.Position.Z * a.Position.X;
this.realY = System.Math.Tan(Math.PI * 21.5 / 180) * a.Position.Z * a.Position.Y;
}
/*private void DrawDebugPanel()
* {
* if (_txtDebug != null)
* {
* int fixtures = 0;
* for (int i = 0; i < World.BodyList.Count; i++)
* {
* fixtures += World.BodyList[i].FixtureList.Count;
* }
*
* StringBuilder output = new StringBuilder();
* output.AppendLine("Bodies: " + World.BodyList.Count);
* output.AppendLine("Fixtures: " + fixtures);
* output.AppendLine("Contacts: " + World.ContactList.Count);
* output.AppendLine("Joints: " + World.JointList.Count);
* output.AppendLine("Proxies: " + World.ProxyCount);
* output.AppendLine("Breakable: " + World.BreakableBodyList.Count);
* output.AppendLine("Controllers: " + World.ControllerList.Count);
*
* output.AppendLine();
*
* output.AppendLine("Controllers: " + World.ControllersUpdateTime);
* output.AppendLine("Contacts: " + World.ContactsUpdateTime);
* output.AppendLine("Solve: " + World.SolveUpdateTime);
* output.AppendLine("CCD: " + World.ContinuousPhysicsTime);
* output.AppendLine("Total: " + World.UpdateTime);
*
* _txtDebug.Text = output.ToString();
* }
* }*/
private void DrawJoint(Joint joint)
{
Body b1 = joint.BodyA;
Body b2 = joint.BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
Vector2 x2 = new Vector2();
// WIP David
if (!joint.IsFixedType())
{
b2.GetTransform(out xf2);
x2 = xf2.Position;
}
Vector2 p2 = joint.WorldAnchorB;
Vector2 x1 = xf1.Position;
Vector2 p1 = joint.WorldAnchorA;
Color color = Color.FromArgb(255, 128, 205, 205);
switch (joint.JointType)
{
case JointType.Distance:
DrawSegment(p1, p2, color);
break;
case JointType.Pulley:
{
PulleyJoint pulley = (PulleyJoint)joint;
Vector2 s1 = pulley.GroundAnchorA;
Vector2 s2 = pulley.GroundAnchorB;
DrawSegment(s1, p1, color);
DrawSegment(s2, p2, color);
DrawSegment(s1, s2, color);
}
break;
case JointType.FixedMouse:
DrawPoint(p1, 0.5f, Color.FromArgb(255, 0, 255, 0));
DrawSegment(p1, p2, Color.FromArgb(255, 205, 205, 205));
break;
case JointType.Revolute:
//DrawSegment(x2, p1, color);
DrawSegment(p2, p1, color);
DrawSolidCircle(p2, 0.1f, new Vector2(), Colors.Red);
DrawSolidCircle(p1, 0.1f, new Vector2(), Colors.Blue);
break;
case JointType.FixedRevolute:
DrawSolidCircle(p1, 0.1f, new Vector2(), Colors.Purple);
break;
case JointType.FixedLine:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.FixedDistance:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.FixedPrismatic:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.Gear:
DrawSegment(x1, x2, color);
//DrawSegment(x1, p1, color);
//DrawSegment(p1, p2, color);
break;
default:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
DrawSegment(x2, p2, color);
break;
}
}
double distance(Joint j1, Joint j2)
{
return(Math.Sqrt(Math.Pow(Math.Abs(j1.Position.X - j2.Position.X), 2) + Math.Pow(Math.Abs(j1.Position.Y - j2.Position.Y), 2) + Math.Pow(Math.Abs(j1.Position.Z - j2.Position.Z), 2)));
}
private void DrawJoint(Joint joint)
{
if (!joint.Enabled)
{
return;
}
Body b1 = joint.BodyA;
Body b2 = joint.BodyB;
Transform xf1;
b1.GetTransform(out xf1);
Vector2 x2 = Vector2.Zero;
// WIP David
if (!joint.IsFixedType())
{
Transform xf2;
b2.GetTransform(out xf2);
x2 = xf2.p;
}
Vector2 p1 = joint.WorldAnchorA;
Vector2 p2 = joint.WorldAnchorB;
Vector2 x1 = xf1.p;
Color color = new Color(0.5f, 0.8f, 0.8f);
switch (joint.JointType)
{
case JointType.Distance:
DrawSegment(p1, p2, color);
break;
case JointType.Pulley:
PulleyJoint pulley = (PulleyJoint)joint;
Vector2 s1 = b1.GetWorldPoint(pulley.LocalAnchorA);
Vector2 s2 = b2.GetWorldPoint(pulley.LocalAnchorB);
DrawSegment(p1, p2, color);
DrawSegment(p1, s1, color);
DrawSegment(p2, s2, color);
break;
case JointType.FixedMouse:
DrawPoint(p1, 0.5f, new Color(0.0f, 1.0f, 0.0f));
DrawSegment(p1, p2, new Color(0.8f, 0.8f, 0.8f));
break;
case JointType.Revolute:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
DrawSegment(x2, p2, color);
DrawSolidCircle(p2, 0.1f, Vector2.Zero, Color.Red);
DrawSolidCircle(p1, 0.1f, Vector2.Zero, Color.Blue);
break;
case JointType.FixedAngle:
//Should not draw anything.
break;
case JointType.FixedRevolute:
DrawSegment(x1, p1, color);
DrawSolidCircle(p1, 0.1f, Vector2.Zero, Color.Pink);
break;
case JointType.FixedLine:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.FixedDistance:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.FixedPrismatic:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.Gear:
DrawSegment(x1, x2, color);
break;
default:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
DrawSegment(x2, p2, color);
break;
}
}
public void Solve(ref TimeStep step, ref Vector2 gravity)
{
float h = step.Dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
var b = Bodies[i];
var c = b._sweep.C;
float a = b._sweep.A;
var v = b._linearVelocity;
float w = b._angularVelocity;
// Store positions for continuous collision.
b._sweep.C0 = b._sweep.C;
b._sweep.A0 = b._sweep.A;
if (b.BodyType == BodyType.Dynamic)
{
// Integrate velocities.
// FPE: Only apply gravity if the body wants it.
if (b.IgnoreGravity)
{
v += h * (b._invMass * b._force);
}
else
{
v += h * (b.GravityScale * gravity + b._invMass * b._force);
}
w += h * b._invI * b._torque;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
}
_positions[i].C = c;
_positions[i].A = a;
_velocities[i].V = v;
_velocities[i].W = w;
}
// Solver data
SolverData solverData = new SolverData();
solverData.Step = step;
solverData.Positions = _positions;
solverData.Velocities = _velocities;
_contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities);
_contactSolver.InitializeVelocityConstraints();
if (Settings.EnableWarmstarting)
{
_contactSolver.WarmStart();
}
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
for (int i = 0; i < JointCount; ++i)
{
if (_joints[i].Enabled)
{
_joints[i].InitVelocityConstraints(ref solverData);
}
}
if (Settings.EnableDiagnostics)
{
_watch.Stop();
}
// Solve velocity constraints.
for (int i = 0; i < Settings.VelocityIterations; ++i)
{
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
joint.SolveVelocityConstraints(ref solverData);
joint.Validate(step.Inv_dt);
if (Settings.EnableDiagnostics)
{
_watch.Stop();
}
}
_contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting.
_contactSolver.StoreImpulses();
// Integrate positions
for (int i = 0; i < BodyCount; ++i)
{
Vector2 c = _positions[i].C;
float a = _positions[i].A;
Vector2 v = _velocities[i].V;
float w = _velocities[i].W;
// Check for large velocities
Vector2 translation = h * v;
if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
float ratio = Settings.MaxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > Settings.MaxRotationSquared)
{
float ratio = Settings.MaxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
_positions[i].C = c;
_positions[i].A = a;
_velocities[i].V = v;
_velocities[i].W = w;
}
// Solve position constraints
bool positionSolved = false;
for (int i = 0; i < Settings.PositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
bool jointOkay = joint.SolvePositionConstraints(ref solverData);
if (Settings.EnableDiagnostics)
{
_watch.Stop();
}
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
if (Settings.EnableDiagnostics)
{
JointUpdateTime = _watch.ElapsedTicks;
_watch.Reset();
}
// Copy state buffers back to the bodies
for (int i = 0; i < BodyCount; ++i)
{
Body body = Bodies[i];
body._sweep.C = _positions[i].C;
body._sweep.A = _positions[i].A;
body._linearVelocity = _velocities[i].V;
body._angularVelocity = _velocities[i].W;
body.SynchronizeTransform();
}
Report(_contactSolver._velocityConstraints);
if (Settings.AllowSleep)
{
float minSleepTime = Settings.MaxFloat;
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
if (!b.IsSleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += h;
minSleepTime = Math.Min(minSleepTime, b._sleepTime);
}
}
if (minSleepTime >= Settings.TimeToSleep && positionSolved)
{
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
b.IsAwake = false;
}
}
}
}
void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
var reference = e.FrameReference.AcquireFrame();
// Color
using (var frame = reference.ColorFrameReference.AcquireFrame())
{
if (frame != null)
{
camera.Source = frame.ToBitmap();
}
}
// Body
using (var frame = reference.BodyFrameReference.AcquireFrame())
{
if (frame != null)
{
canvas.Children.Clear();
_bodies = new Body[frame.BodyFrameSource.BodyCount];
frame.GetAndRefreshBodyData(_bodies);
#region drawing_ranad
Rectangle ranad_teeth1 = new Rectangle();
DrawingRanadTeeth(ranad_teeth1, 30, 200, 120, 600);
canvas.Children.Add(ranad_teeth1);
Rectangle ranad_teeth2 = new Rectangle();
DrawingRanadTeeth(ranad_teeth2, 30, 200, 160, 600);
canvas.Children.Add(ranad_teeth2);
Rectangle ranad_teeth3 = new Rectangle();
DrawingRanadTeeth(ranad_teeth3, 30, 200, 200, 600);
canvas.Children.Add(ranad_teeth3);
Rectangle ranad_teeth4 = new Rectangle();
DrawingRanadTeeth(ranad_teeth4, 30, 200, 240, 600);
canvas.Children.Add(ranad_teeth4);
Rectangle ranad_teeth5 = new Rectangle();
DrawingRanadTeeth(ranad_teeth5, 30, 200, 280, 600);
canvas.Children.Add(ranad_teeth5);
Rectangle ranad_teeth6 = new Rectangle();
DrawingRanadTeeth(ranad_teeth6, 30, 200, 320, 600);
canvas.Children.Add(ranad_teeth6);
Rectangle ranad_teeth7 = new Rectangle();
DrawingRanadTeeth(ranad_teeth7, 30, 200, 360, 600);
canvas.Children.Add(ranad_teeth7);
Rectangle ranad_teeth8 = new Rectangle();
DrawingRanadTeeth(ranad_teeth8, 30, 200, 400, 600);
canvas.Children.Add(ranad_teeth8);
Rectangle ranad_teeth9 = new Rectangle();
DrawingRanadTeeth(ranad_teeth9, 30, 200, 440, 600);
canvas.Children.Add(ranad_teeth9);
Rectangle ranad_teeth10 = new Rectangle();
DrawingRanadTeeth(ranad_teeth10, 30, 200, 480, 600);
canvas.Children.Add(ranad_teeth10);
Rectangle ranad_teeth11 = new Rectangle();
DrawingRanadTeeth(ranad_teeth11, 30, 200, 520, 600);
canvas.Children.Add(ranad_teeth11);
Rectangle ranad_teeth12 = new Rectangle();
DrawingRanadTeeth(ranad_teeth12, 30, 200, 560, 600);
canvas.Children.Add(ranad_teeth12);
Rectangle ranad_teeth13 = new Rectangle();
DrawingRanadTeeth(ranad_teeth13, 30, 200, 600, 600);
canvas.Children.Add(ranad_teeth13);
Rectangle ranad_teeth14 = new Rectangle();
DrawingRanadTeeth(ranad_teeth14, 30, 200, 640, 600);
canvas.Children.Add(ranad_teeth14);
Rectangle ranad_teeth15 = new Rectangle();
DrawingRanadTeeth(ranad_teeth15, 30, 200, 680, 600);
canvas.Children.Add(ranad_teeth15);
Rectangle ranad_teeth16 = new Rectangle();
DrawingRanadTeeth(ranad_teeth16, 30, 200, 720, 600);
canvas.Children.Add(ranad_teeth16);
Rectangle ranad_teeth17 = new Rectangle();
DrawingRanadTeeth(ranad_teeth17, 30, 200, 760, 600);
canvas.Children.Add(ranad_teeth17);
Rectangle ranad_teeth18 = new Rectangle();
DrawingRanadTeeth(ranad_teeth18, 30, 200, 800, 600);
canvas.Children.Add(ranad_teeth18);
Rectangle ranad_teeth19 = new Rectangle();
DrawingRanadTeeth(ranad_teeth19, 30, 200, 840, 600);
canvas.Children.Add(ranad_teeth19);
Rectangle ranad_teeth20 = new Rectangle();
DrawingRanadTeeth(ranad_teeth20, 30, 200, 880, 600);
canvas.Children.Add(ranad_teeth20);
Rectangle ranad_teeth21 = new Rectangle();
DrawingRanadTeeth(ranad_teeth21, 30, 200, 920, 600);
canvas.Children.Add(ranad_teeth21);
Rectangle ranad_teeth22 = new Rectangle();
DrawingRanadTeeth(ranad_teeth22, 30, 200, 960, 600);
canvas.Children.Add(ranad_teeth22);
#endregion
#region drawing_circle_example
//Draw the drum
//Ellipse circle1 = new Ellipse();
//circle1.StrokeThickness = 5;
//circle1.Stroke = Brushes.Black;
//circle1.Width = 140;
//circle1.Height = 55;
//Canvas.SetLeft(circle1, 120);
//Canvas.SetTop(circle1, 600);
//canvas.Children.Add(circle1);
//Ellipse circle2 = new Ellipse();
//circle2.StrokeThickness = 5;
//circle2.Stroke = Brushes.Black;
//circle2.Width = 180;
//circle2.Height = 70;
//Canvas.SetLeft(circle2, 270);
//Canvas.SetTop(circle2, 605);
//canvas.Children.Add(circle2);
//Ellipse circle3 = new Ellipse();
//circle3.StrokeThickness = 5;
//circle3.Stroke = Brushes.Black;
//circle3.Width = 180;
//circle3.Height = 70;
//Canvas.SetLeft(circle3, 410);
//Canvas.SetTop(circle3, 620);
//canvas.Children.Add(circle3);
#endregion
foreach (var body in _bodies)
{
if (body != null)
{
if (body.IsTracked)
{
// Find the joints
Joint handRight = body.Joints[JointType.HandRight];
Joint thumbRight = body.Joints[JointType.ThumbRight];
double rightX = handRight.Position.X;
double rightY = handRight.Position.Y;
Joint handLeft = body.Joints[JointType.HandLeft];
Joint thumbLeft = body.Joints[JointType.ThumbLeft];
double leftX = handLeft.Position.X * (-100);
double leftY = handLeft.Position.Y * 100;
double leftZ = handLeft.Position.Z * 100;
// Draw hands and thumbs
canvas.DrawHand(handRight, _sensor.CoordinateMapper);
canvas.DrawHand(handLeft, _sensor.CoordinateMapper);
//canvas.DrawThumb(thumbRight, _sensor.CoordinateMapper);
//canvas.DrawThumb(thumbLeft, _sensor.CoordinateMapper);
double hitFirstMax = 70;
double hitFirstMin = 60;
//set direction
double direction = 1;
if (leftY > 5)
{
direction = 1;
}
else
{
direction = 0;
}
// Find the hand states
string rightHandState = "-";
string leftHandState = "-";
rightHandState = System.Convert.ToString(leftX);
leftHandState = System.Convert.ToString(leftY);
tblRightHandState.Text = rightHandState;
tblLeftHandState.Text = leftHandState;
if ((leftX <= 107) && (leftX >= 102) && ((leftY >= 7.9) && (leftY <= 10.1)))
{
System.Media.SoundPlayer startSound = new System.Media.SoundPlayer(@"C:\Users\USER\Desktop\project thesis kinect\sound\air.wav");
if (direction == 1)
{
startSound.Play();
}
}
else if ((leftX <= 50) && (leftX >= 40) && ((leftY >= 7.9) && (leftY <= 10.1)))
{
System.Media.SoundPlayer midSound = new System.Media.SoundPlayer(@"C:\Users\USER\Desktop\project thesis kinect\sound\bullet.wav");
if (direction == 1)
{
midSound.Play();
}
}
else if ((leftX <= 30) && (leftX >= 20) && ((leftY >= 7.9) && (leftY <= 10.1)))
{
System.Media.SoundPlayer test = new System.Media.SoundPlayer(@"C:\Users\USER\Desktop\project thesis kinect\sound\cry.wav");
if (direction == 1)
{
test.Play();
}
}
}
}
}
}
}
}
// *** Only StoreApp with WindowsPreview.Kinect;
// private void msfr_MultiSourceFrameArrived(MultiSourceFrameReader sender, MultiSourceFrameArrivedEventArgs args)
private void msfr_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
// using (MultiSourceFrame msf = e.FrameReference.AcquireFrame()) *** Only StoreApp with WindowsPreview.Kinect;
// {
MultiSourceFrame msf = e.FrameReference.AcquireFrame();
if (msf != null)
{
using (BodyFrame bodyframe = msf.BodyFrameReference.AcquireFrame())
{
using (InfraredFrame irf = msf.InfraredFrameReference.AcquireFrame())
{
if (bodyframe != null && irf != null)
{
/* *** only with Windows.UI.Xaml.Media.Imaging;
* irf.CopyFrameDataToArray(irData);
* for (int i = 0; i < irData.Length; i++)
* {
*
* byte intensity = (byte)(irData[i] >> 8);
* irDataConverted[i * 4] = intensity;
* irDataConverted[i * 4 + 1] = intensity;
* irDataConverted[i * 4 + 2] = intensity;
* irDataConverted[i * 4 + 3] = 255;
* }
*
* irDataConverted.CopyTo(irBitmap.PixelBuffer);
* irBitmap.Invalidate();
*/
// The below is from Kinect Studio WPF infrared sample
// irf.CopyFrameDataToArray(irData);
using (Microsoft.Kinect.KinectBuffer infraredBuffer = irf.LockImageBuffer())
{
// verify data and write the new infrared frame data to the display bitmap
if (((this.fd.Width * this.fd.Height) == (infraredBuffer.Size / this.fd.BytesPerPixel)) &&
(this.fd.Width == this.irBitmap.PixelWidth) && (this.fd.Height == this.irBitmap.PixelHeight))
{
this.ProcessInfraredFrameData(infraredBuffer.UnderlyingBuffer, infraredBuffer.Size);
}
}
bodyframe.GetAndRefreshBodyData(bodies);
bodyCanvas.Children.Clear();
foreach (Body b in bodies)
{
if (b.IsTracked)
{
Joint hand = b.Joints[JointType.HandLeft];
if (hand.TrackingState == TrackingState.Tracked)
{
DepthSpacePoint dsp = ksensor.CoordinateMapper.MapCameraPointToDepthSpace(hand.Position);
var circle = CreateCircle(dsp);
tbox.Content = "x:" + (dsp.X / 2).ToString() + " y" + (dsp.Y / 2).ToString();
// bodyCanvas.Children.Add(circle);
DetectPageTurn(dsp, circle);
// Canvas.SetLeft(circle, dsp.X);
//Canvas.SetTop(circle, dsp.Y);
}
}
}
}
}
}
msf = null;
}
//}
}
// This is the main function for detecting the gesture.
// It checks the position of all of the joints and compare them together,
// then it decides to update which one one of them.
// If it can not categorized the the position five times, it will reset all of the variables.
private void TrackGesture(Skeleton skeleton, int index, long timestamp)
{
JointType leftHandJointId = JointType.HandLeft;
JointType rightHandJointId = JointType.HandRight;
JointType leftWristId = JointType.WristLeft;
JointType rightWristId = JointType.WristRight;
JointType leftElbowJointId = JointType.ElbowLeft;
JointType rightElbowJointId = JointType.ElbowRight;
JointType leftShoulderId = JointType.ShoulderLeft;
JointType rightShoulderId = JointType.ShoulderRight;
JointType leftHipJointId = JointType.HipLeft;
JointType leftKneeJointId = JointType.KneeLeft;
JointType leftFootJointId = JointType.FootLeft;
JointType rightHipJointId = JointType.HipRight;
JointType rightKneeJointId = JointType.KneeRight;
JointType rightFootJointId = JointType.FootRight;
Joint leftHand = skeleton.Joints[leftHandJointId];
Joint rightHand = skeleton.Joints[rightHandJointId];
Joint leftWrist = skeleton.Joints[leftWristId];
Joint rightWrist = skeleton.Joints[rightWristId];
Joint leftElbow = skeleton.Joints[leftElbowJointId];
Joint rightElbow = skeleton.Joints[rightElbowJointId];
Joint leftShoulder = skeleton.Joints[leftShoulderId];
Joint rightShoulder = skeleton.Joints[rightShoulderId];
Joint leftHip = skeleton.Joints[leftHipJointId];
Joint leftKnee = skeleton.Joints[leftKneeJointId];
Joint leftFoot = skeleton.Joints[leftFootJointId];
Joint rightHip = skeleton.Joints[rightHipJointId];
Joint rightKnee = skeleton.Joints[rightKneeJointId];
Joint rightFoot = skeleton.Joints[rightFootJointId];
if (leftShoulder.TrackingState != JointTrackingState.NotTracked &&
rightShoulder.TrackingState != JointTrackingState.NotTracked &&
leftElbow.TrackingState != JointTrackingState.NotTracked &&
rightElbow.TrackingState != JointTrackingState.NotTracked &&
leftWrist.TrackingState != JointTrackingState.NotTracked &&
rightWrist.TrackingState != JointTrackingState.NotTracked &&
leftHand.TrackingState != JointTrackingState.NotTracked &&
rightHand.TrackingState != JointTrackingState.NotTracked)
{
// This part is responsible for left hand wave and punch compeletly and for the start position of turn down volume.
#region Left Hand Wave and Punch
if (leftHand.Position.Y > leftElbow.Position.Y &&
leftHand.Position.Y > leftShoulder.Position.Y)
{
this._Categorized = true;
// left hand wave
if (timestamp > this._WaveTracker[index, 0].timeStamp + timeOut &&
this._WaveTracker[index, 0].state == GestureState.InProgress)
{
this._WaveTracker[index, 0].UpdateState(GestureState.Failure, timestamp);
}
else if (leftHand.Position.X <= leftElbow.Position.X - threshold)
{
this._WaveTracker[index, 0].UpdateWave(GesturePosition.Start, timestamp);
}
else if (leftHand.Position.X >= leftElbow.Position.X + threshold)
{
this._WaveTracker[index, 0].UpdateWave(GesturePosition.End, timestamp);
}
if (this._WaveTracker[index, 0].state != GestureState.Success && this._WaveTracker[index, 0].counter == waveItrations)
{
this._WaveTracker[index, 0].UpdateState(GestureState.Success, timestamp);
if (GestureDetected != null)
{
this.Gesture = 1;
GestureDetected(this, new EventArgs());
this.Reset(index);
}
}
// Left Hand Punch
if (timestamp > this._PunchTracker[index, 0].timeStamp + timeOut)
//this._PunchTracker[index, 0].state == GestureState.InProgress)
{
this._PunchTracker[index, 0].UpdateState(GestureState.Failure, timestamp);
}
else if (//leftHand.Position.X <= leftElbow.Position.X && // - threshold &&
Math.Abs(leftHand.Position.Z - leftShoulder.Position.Z) <= 2.5 * threshold &&
this._PunchTracker[index, 0].state != GestureState.InProgress)
{
this._PunchTracker[index, 0].UpdatePosition(GesturePosition.Start, timestamp);
}
else if (//leftHand.Position.X <= leftElbow.Position.X &&// - threshold &&
Math.Abs(leftHand.Position.Z - leftShoulder.Position.Z) >= 3.5 * threshold &&
this._PunchTracker[index, 0].state == GestureState.InProgress)
{
this._PunchTracker[index, 0].UpdatePosition(GesturePosition.End, timestamp);
}
if (this._PunchTracker[index, 0].state != GestureState.Success && this._PunchTracker[index, 0].counter == generalItrations)
{
this._PunchTracker[index, 0].UpdateState(GestureState.Success, timestamp);
if (GestureDetected != null)
{
this.Gesture = 7;
GestureDetected(this, new EventArgs());
this.Reset(index);
}
}
// Start for Turn down volume
if (leftHand.Position.Y > leftElbow.Position.Y + threshold &&
this._UpDownTracker[index, 0].state != GestureState.InProgress)
{
this._UpDownTracker[index, 0].UpdatePosition(GesturePosition.Start, timestamp);
}
}
#endregion Left Hand Wave and Punch
// This part is responsible for right hand wave and punch compeletly and for the end position of turn up volume.
#region Right Hand Wave and Punch
if (rightHand.Position.Y > rightElbow.Position.Y &&
rightHand.Position.Y > rightShoulder.Position.Y)
{
this._Categorized = true;
// right hand wave
if (timestamp > this._WaveTracker[index, 1].timeStamp + timeOut &&
this._WaveTracker[index, 1].state == GestureState.InProgress)
{
this._WaveTracker[index, 1].UpdateState(GestureState.Failure, timestamp);
}
else if (rightHand.Position.X >= rightElbow.Position.X + threshold)
{
this._WaveTracker[index, 1].UpdateWave(GesturePosition.Start, timestamp);
}
else if (rightHand.Position.X <= rightElbow.Position.X - threshold)
{
this._WaveTracker[index, 1].UpdateWave(GesturePosition.End, timestamp);
}
if (this._WaveTracker[index, 1].state != GestureState.Success && this._WaveTracker[index, 1].counter == waveItrations)
{
this._WaveTracker[index, 1].UpdateState(GestureState.Success, timestamp);
if (GestureDetected != null)
{
this.Gesture = 2;
GestureDetected(this, new EventArgs());
this.Reset(index);
}
}
// Right Hand Punch
if (timestamp > this._PunchTracker[index, 1].timeStamp + timeOut)
//this._PunchTracker[index, 1].state == GestureState.InProgress)
{
this._PunchTracker[index, 1].UpdateState(GestureState.Failure, timestamp);
}
else if (//rightHand.Position.X <= rightElbow.Position.X && // - threshold &&
Math.Abs(rightHand.Position.Z - rightShoulder.Position.Z) <= 2.5 * threshold &&
this._PunchTracker[index, 1].state != GestureState.InProgress)
{
this._PunchTracker[index, 1].UpdatePosition(GesturePosition.Start, timestamp);
}
else if (//rightHand.Position.X <= leftElbow.Position.X &&// - threshold &&
Math.Abs(rightHand.Position.Z - rightShoulder.Position.Z) >= 3.5 * threshold &&
this._PunchTracker[index, 1].state == GestureState.InProgress)
{
this._PunchTracker[index, 1].UpdatePosition(GesturePosition.End, timestamp);
}
if (this._PunchTracker[index, 1].state != GestureState.Success && this._PunchTracker[index, 1].counter == generalItrations)
{
this._PunchTracker[index, 1].UpdateState(GestureState.Success, timestamp);
if (GestureDetected != null)
{
this.Gesture = 8;
GestureDetected(this, new EventArgs());
this.Reset(index);
}
}
// End for Turn Up volume
if (rightHand.Position.Y > rightElbow.Position.Y + threshold &&
this._UpDownTracker[index, 1].state == GestureState.InProgress)
{
if (this._UpDownTracker[index, 1].timeStamp + timeOut < timestamp)
{
this._UpDownTracker[index, 1].UpdateState(GestureState.Failure, timestamp);
}
else
{
this._UpDownTracker[index, 1].UpdatePosition(GesturePosition.End, timestamp);
}
}
if (this._UpDownTracker[index, 1].state != GestureState.Success && this._UpDownTracker[index, 1].counter == generalItrations)
{
this._UpDownTracker[index, 1].UpdateState(GestureState.Success, timestamp);
if (GestureDetected != null)
{
this.Gesture = 6;
GestureDetected(this, new EventArgs());
this.Reset(index);
}
}
}
#endregion Right Hand Wave and Punch
// This part is responsible for left hand swipe compeletly
#region Left Hand Swipe
if (leftShoulder.Position.Y > leftHand.Position.Y && // left hand swipe
leftHand.Position.Y > leftElbow.Position.Y) //+ threshold)
{
// Left Hand Swipe
this._Categorized = true;
if (this._SwipeTracker[index, 0].timeStamp + timeOut < timestamp &&
this._SwipeTracker[index, 0].state == GestureState.InProgress)
{
this._SwipeTracker[index, 0].UpdateState(GestureState.Failure, timestamp);
}
else if (leftHand.Position.X <= leftElbow.Position.X - threshold &&
this._SwipeTracker[index, 0].state != GestureState.InProgress)
{
this._SwipeTracker[index, 0].UpdatePosition(GesturePosition.Start, timestamp);
}
else if (leftHand.Position.X >= leftElbow.Position.X + threshold &&
this._SwipeTracker[index, 0].state == GestureState.InProgress)
{
this._SwipeTracker[index, 0].UpdatePosition(GesturePosition.End, timestamp);
}
if (this._SwipeTracker[index, 0].state != GestureState.Success && this._SwipeTracker[index, 0].counter == generalItrations)
{
this._SwipeTracker[index, 0].UpdateState(GestureState.Success, timestamp);
if (GestureDetected != null)
{
this.Gesture = 3;
GestureDetected(this, new EventArgs());
this.Reset(index);
}
}
}
#endregion Left Hand Swipe
// This part is responsible for right hand swipe and Stop compeletly
#region Right Hand Swipe and Stop
//Right hand swipe
if (rightShoulder.Position.Y > rightHand.Position.Y && // right hand swipe
rightHand.Position.Y > rightElbow.Position.Y) // + threshold)
{
this._Categorized = true;
if (this._SwipeTracker[index, 1].timeStamp + timeOut < timestamp &&
this._SwipeTracker[index, 1].state == GestureState.InProgress)
{
this._SwipeTracker[index, 1].UpdateState(GestureState.Failure, timestamp);
}
else if (rightHand.Position.X >= rightElbow.Position.X + threshold &&
this._SwipeTracker[index, 1].state != GestureState.InProgress)
{
this._SwipeTracker[index, 1].UpdatePosition(GesturePosition.Start, timestamp);
}
else if (rightHand.Position.X <= rightElbow.Position.X - threshold &&
this._SwipeTracker[index, 1].state == GestureState.InProgress)
{
this._SwipeTracker[index, 1].UpdatePosition(GesturePosition.End, timestamp);
}
if (this._SwipeTracker[index, 1].state != GestureState.Success && this._SwipeTracker[index, 1].counter == generalItrations)
{
this._SwipeTracker[index, 1].UpdateState(GestureState.Success, timestamp);
if (GestureDetected != null)
{
this.Gesture = 4;
GestureDetected(this, new EventArgs());
this.Reset(index);
}
}
// Stop
if (this._StopTracker[index].timeStamp + timeOut < timestamp)
{
this._StopTracker[index].UpdateState(GestureState.Failure, timestamp);
}
else if (this._StopTracker[index].state != GestureState.InProgress)
{
this._StopTracker[index].UpdatePosition(GesturePosition.Start, timestamp);
}
else if (this._StopTracker[index].state == GestureState.InProgress &&
this._StopTracker[index].currentPosition != GesturePosition.End)
{
this._StopTracker[index].UpdatePosition(GesturePosition.End, timestamp);
}
else if (this._StopTracker[index].state == GestureState.InProgress &&
this._StopTracker[index].currentPosition != GesturePosition.Middle)
{
this._StopTracker[index].UpdatePosition(GesturePosition.Middle, timestamp);
}
if (this._StopTracker[index].state != GestureState.Success && this._StopTracker[index].counter == 90)
{
this._StopTracker[index].UpdateState(GestureState.Success, timestamp);
if (GestureDetected != null)
{
this.Gesture = 11;
GestureDetected(this, new EventArgs());
this.Reset(index);
}
}
}
#endregion Right Hand Swipe and Stop
// This part is responsible for the end part of turn down and the start of turn up.
#region Up Dwon
if (leftShoulder.Position.Y > leftElbow.Position.Y &&
leftElbow.Position.Y > leftHand.Position.Y)
{
// End for Turn Down volume
if (leftHand.Position.X < leftElbow.Position.X)
{
this._Categorized = true;
if (this._UpDownTracker[index, 0].state == GestureState.InProgress)
{
if (this._UpDownTracker[index, 0].timeStamp + timeOut < timestamp)
{
this._UpDownTracker[index, 0].UpdateState(GestureState.Failure, timestamp);
}
else
{
this._UpDownTracker[index, 0].UpdatePosition(GesturePosition.End, timestamp);
}
}
if (this._UpDownTracker[index, 0].state != GestureState.Success && this._UpDownTracker[index, 0].counter == generalItrations)
{
this._UpDownTracker[index, 0].UpdateState(GestureState.Success, timestamp);
if (GestureDetected != null)
{
this.Gesture = 5;
GestureDetected(this, new EventArgs());
this.Reset(index);
}
}
}
}
if (rightShoulder.Position.Y > rightElbow.Position.Y &&
rightElbow.Position.Y > rightHand.Position.Y)
{
// Start for Turn Up Volume
if (rightElbow.Position.X < rightHand.Position.X)
{
this._Categorized = true;
if (this._UpDownTracker[index, 1].timeStamp + timeOut < timestamp)
{
this._UpDownTracker[index, 1].UpdateState(GestureState.Failure, timestamp);
}
else if (this._UpDownTracker[index, 1].state != GestureState.InProgress)
{
this._UpDownTracker[index, 1].UpdatePosition(GesturePosition.Start, timestamp);
}
}
}
#endregion Up Down
// This part is responsible for detection of the kick gesture
#region Kick
if (leftShoulder.Position.Y > leftElbow.Position.Y && // kick and ...
leftElbow.Position.Y > leftHand.Position.Y &&
rightShoulder.Position.Y > rightElbow.Position.Y &&
rightElbow.Position.Y > rightHand.Position.Y)
{
this._Categorized = true;
for (int i = 0; i < 2; i++)
{
Joint knee;
Joint foot;
if (i == 0)
{
knee = leftKnee;
foot = leftFoot;
}
else
{
knee = rightKnee;
foot = rightFoot;
}
if (this._KickTracker[index, i].timeStamp + timeOut < timestamp &&
this._KickTracker[index, i].state == GestureState.InProgress)
{
this._KickTracker[index, i].UpdateState(GestureState.Failure, timestamp);
}
else if (Math.Abs(knee.Position.X - foot.Position.X) <= threshold)
{
if (foot.Position.Z >= knee.Position.Z + threshold)
{
this._KickTracker[index, i].UpdatePosition(GesturePosition.Start, timestamp);
}
else if (foot.Position.Z <= knee.Position.Z - threshold)
{
this._KickTracker[index, i].UpdatePosition(GesturePosition.End, timestamp);
}
else
{
this._KickTracker[index, i].UpdatePosition(GesturePosition.Middle, timestamp);
}
if (this._KickTracker[index, i].state != GestureState.Success && this._KickTracker[index, i].counter == generalItrations)
{
this._KickTracker[index, i].UpdateState(GestureState.Success, timestamp);
if (this.GestureDetected != null)
{
this.Gesture = (i == 0) ? 9 : 10;
GestureDetected(this, new EventArgs());
}
}
}
}
}
#endregion Kick
if (this._Categorized)
{
this._Categorized = false;
}
else
{
if (this._ResetCounter < 5)
{
this._ResetCounter++;
}
else
{
this._ResetCounter = 0;
this.Reset(index);
}
}
}
else
{
if (this._ResetCounter < 5)
{
this._ResetCounter++;
}
else
{
this._ResetCounter = 0;
this.Reset(index);
}
}
}
/// <summary>
/// Scales the specified joint according to the specified dimensions.
/// </summary>
/// <param name="joint">The joint to scale.</param>
/// <param name="width">Width.</param>
/// <param name="height">Height.</param>
/// <returns>The scaled version of the joint.</returns>
public static Joint ScaleTo(this Joint joint, double width, double height)
{
return(ScaleTo(joint, width, height, 1.0f, 1.0f));
}
/// <summary>
/// Returns the distance of the specified joints.
/// </summary>
/// <param name="p1">The first joint (start of the segment).</param>
/// <param name="p2">The second joint (end of the segment).</param>
/// <returns>The length of the segment in meters.</returns>
public static double DistanceFrom(this Joint p1, Joint p2)
{
return(Distance(p1, p2));
}
private void DealWith()
{
try
{
Console.WriteLine("Update");
Nuitrack.Update(_skeletonTracker);
}
catch (LicenseNotAcquiredException exception)
{
Console.WriteLine("LicenseNotAcquired exception. Exception: ", exception);
MessageBoxX.Error("3D摄像头认证异常，请重新进入页面重试。");
//throw exception;
}
catch (nuitrack.Exception exception)
{
Console.WriteLine("Nuitrack update failed. Exception: ", exception);
MessageBoxX.Error("3D摄像头更新异常，请重新进入页面重试。");
}
// Draw skeleton joints
//CQZ:检测骨架关节数据,骨骼数组不为空开始，里边while循环判断骨骼数组里如果为空，就调用API更新骨骼数据，线程sleep 0.1秒再循环检测
while (true)
{
if (_skeletonData != null)
{
//加try catch处理 --CQZ
try
{
Console.WriteLine("_skeletonData不为空，捕捉到人");
while (_skeletonData.Skeletons.Length == 0)
{
Nuitrack.Update(_skeletonTracker);
Console.WriteLine("Skeletons为空进行Update,Skeletons是个数组，每个元素对应一个人");
Thread.Sleep(100);
}
}
catch
{
Console.WriteLine("Skeletons为空进行Update异常");
MessageBoxX.Error("骨骼数据为空，请站到指定位置再开始扫描。");
}
Joint Head = new Joint(); //衣领位置
Joint Collar = new Joint(); //衣领位置
Joint LeftShoulder = new Joint(); //左肩关节
Joint LeftElbow = new Joint(); //左胳膊肘
Joint LeftWrist = new Joint(); //左手手腕关节
Joint LeftHip = new Joint(); //左大腿关节
Joint LeftKnee = new Joint(); //左膝盖关节
Joint LeftAnkle = new Joint(); //左脚踝
Joint Waist = new Joint(); //腰部
Console.WriteLine("Joints长度为" + _skeletonData.Skeletons[0].Joints.Length);
//骨骼关节数据不为空后开始进行图像渲染，各个关节的计算初始化等操作。
try
{
while (!breakFlag)
{ //_skeletonData.Skeletons[0].Joints表示关节 _skeletonData.Skeletons.Length > 0 &&是不是可以删掉
if (_skeletonData.Skeletons.Length > 0 && _skeletonData.Skeletons[0].Joints.Length > 0)
{
Console.WriteLine("外层循环");
//CQZ:一直以来罪魁祸首崩溃异常的大坑！数组越界bug修改!!!!不应该是i < [].length 应该是 i < [].length-1
for (int i = 0; i < _skeletonData.Skeletons[0].Joints.Length - 1; i++)
{
Skeleton skeleton = _skeletonData.Skeletons[0];
this.Dispatcher.Invoke(DispatcherPriority.Normal, (ThreadStart) delegate()
{
//描点
Console.WriteLine("图像渲染前" + DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss:fff"));
DrawingVisual drawingVisual = new DrawingVisual();
DrawingContext drawingContext = drawingVisual.RenderOpen();
System.Windows.Media.Brush brush = new SolidColorBrush(System.Windows.Media.Color.FromRgb(255, 0, 0));
foreach (var joint in skeleton.Joints)
{
drawingContext.DrawEllipse(brush, new System.Windows.Media.Pen(), new System.Windows.Point((joint.Proj.X * _bitmap.Width - 10 / 2) - 53, (joint.Proj.Y * _bitmap.Height - 10 / 2) - 70), 5, 5);
}
drawingContext.Close();
RenderTargetBitmap bmp = new RenderTargetBitmap(640, 480, 120, 120, PixelFormats.Pbgra32);
bmp.Render(drawingVisual);
statusImage1.Source = bmp;
Console.WriteLine("图像渲染后" + DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss:fff"));
});
//Console.WriteLine("类型" + i + " " + skeleton.Joints[i].Type.ToString());
if (skeleton.Joints[i].Type.ToString() == "Head")
{
Head = skeleton.Joints[i];
// Console.WriteLine("头部位置坐标" + i + "||" + Head.Real.X + "||" + Head.Real.Y + "||" + Head.Real.Z);
}
if (skeleton.Joints[i].Type.ToString() == "LeftCollar")
{
Collar = skeleton.Joints[i];
// Console.WriteLine("衣领位置坐标" + i + "||" + Collar.Real.X + "||" + Collar.Real.Y + "||" + Collar.Real.Z);
}
if (skeleton.Joints[i].Type.ToString() == "LeftShoulder")
{
LeftShoulder = skeleton.Joints[i];
// Console.WriteLine("左肩关节坐标" + i + "||" + LeftShoulder.Real.X + "||" + LeftShoulder.Real.Y + "||" + LeftShoulder.Real.Z);
}
if (skeleton.Joints[i].Type.ToString() == "LeftWrist")
{
LeftWrist = skeleton.Joints[i];
//Console.WriteLine("左手手腕关节坐标" + i + "||" + LeftWrist.Real.X + "||" + LeftWrist.Real.Y + "||" + LeftWrist.Real.Z);
}
if (skeleton.Joints[i].Type.ToString() == "LeftHip")
{
LeftHip = skeleton.Joints[i];
// Console.WriteLine("左大腿关节坐标" + i + "||" + LeftHip.Real.X + "||" + LeftHip.Real.Y + "||" + LeftHip.Real.Z);
}
if (skeleton.Joints[i].Type.ToString() == "LeftAnkle")
{
LeftAnkle = skeleton.Joints[i];
// Console.WriteLine("左脚踝坐标" + i + "||" + LeftAnkle.Real.X + "||" + LeftAnkle.Real.Y + "||" + LeftAnkle.Real.Z);
}
if (skeleton.Joints[i].Type.ToString() == "LeftElbow")
{
LeftElbow = skeleton.Joints[i];
// Console.WriteLine("左胳膊肘坐标" + i + "||" + LeftElbow.Real.X + "||" + LeftElbow.Real.Y + "||" + LeftElbow.Real.Z);
}
if (skeleton.Joints[i].Type.ToString() == "LeftKnee")
{
LeftKnee = skeleton.Joints[i];
//Console.WriteLine("左膝盖坐标" + i + "||" + LeftKnee.Real.X + "||" + LeftKnee.Real.Y + "||" + LeftKnee.Real.Z);
}
if (skeleton.Joints[i].Type.ToString() == "Waist")
{
Waist = skeleton.Joints[i];
//Console.WriteLine("腰部坐标" + i + "||" + Waist.Real.X + "||" + Waist.Real.Y + "||" + Waist.Real.Z);
}
//因为手动抓拍功能 改为全局变量 CQZ
//计算距离
NeckLength = ComputeDistanceBetween2Joints(Head, Collar);
ShoulderWidth = ComputeDistanceBetween2Joints(LeftShoulder, Collar);
ArmLengthUp = ComputeDistanceBetween2Joints(LeftShoulder, LeftElbow);
ArmLengthDown = ComputeDistanceBetween2Joints(LeftElbow, LeftWrist);
LegLengthUp = ComputeDistanceBetween2Joints(LeftHip, LeftKnee);
LegLengthDown = ComputeDistanceBetween2Joints(LeftKnee, LeftAnkle);
BodyLength = ComputeDistanceBetween2Joints(Collar, Waist);
//获取权重
string initWeight = ConfigUtil.Get("initWeight");
//CQZ:身高计算：各个骨骼相加再补常量 常量请根据测试情况调节
Height = LegLengthUp + LegLengthDown + BodyLength + NeckLength + Double.Parse(initWeight);
//更新，直到点击手动拍照按钮。
Console.WriteLine("Update前");
Nuitrack.Update(_skeletonTracker);
Console.WriteLine("Update后");
}
}
else
{
Nuitrack.Update(_skeletonTracker);
Console.WriteLine("未捕获到关节，更新");
}
}
break;
}
catch (IndexOutOfRangeException)
{
//如果调用release应该会抛出异常并被catch捕获，在此break；
Console.WriteLine("3D扫描数组越界。");
MessageBoxX.Error("扫描失败，请重置后再点击开始重新扫描。");
}
}
else
{
Nuitrack.Update(_skeletonTracker);
Console.WriteLine("没有检测到人，进行Update");
}
}//如果_skeletonData为空，并没有进行处理，就会出现只有一帧画面就停止的状况。使用while(true)包围加上else{update}
}
public unsafe void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
{
var chunkJoint = chunk.GetNativeArray(JointComponentType);
var chunkEntities = chunk.GetNativeArray(EntityType);
int instanceCount = chunk.Count;
for (int i = 0; i < instanceCount; i++)
{
PhysicsJoint joint = chunkJoint[i];
Assert.IsTrue(joint.EntityA != joint.EntityB);
// TODO find a reasonable way to look up the constraint body indices
// - stash body index in a component on the entity? But we don't have random access to Entity data in a job
// - make a map from entity to rigid body index? Sounds bad and I don't think there is any NativeArray-based map data structure yet
// If one of the entities is null, use the default static entity
var pair = new BodyIndexPair
{
BodyAIndex = joint.EntityA == Entity.Null ? DefaultStaticBodyIndex : -1,
BodyBIndex = joint.EntityB == Entity.Null ? DefaultStaticBodyIndex : -1
};
// Find the body indices
for (int bodyIndex = 0; bodyIndex < RigidBodies.Length; bodyIndex++)
{
if (joint.EntityA != Entity.Null)
{
if (RigidBodies[bodyIndex].Entity == joint.EntityA)
{
pair.BodyAIndex = bodyIndex;
if (pair.BodyBIndex >= 0)
{
break;
}
}
}
if (joint.EntityB != Entity.Null)
{
if (RigidBodies[bodyIndex].Entity == joint.EntityB)
{
pair.BodyBIndex = bodyIndex;
if (pair.BodyAIndex >= 0)
{
break;
}
}
}
}
Assert.IsTrue(pair.BodyAIndex != -1 && pair.BodyBIndex != -1);
Joints[firstEntityIndex + i] = new Joint
{
JointData = (JointData *)joint.JointData.GetUnsafePtr(),
BodyPair = pair,
Entity = chunkEntities[i],
EnableCollision = joint.EnableCollision,
};
}
}
private double ComputeDistanceBetween2Joints(Joint Joint1, Joint Joint2)
{
return(Math.Sqrt(Math.Pow(Joint1.Real.X - Joint2.Real.X, 2) + Math.Pow(Joint1.Real.Y - Joint2.Real.Y, 2) + Math.Pow(Joint1.Real.Z - Joint2.Real.Z, 2)));
}
public void Add(Joint joint)
{
Debug.Assert(this.JointCount < this._jointCapacity);
this._joints[this.JointCount++] = joint;
}
/// <summary>
/// Checks for click / touch events and handles them appropiately depending on the
/// currently selected body part.
/// </summary>
private void HandleClicks()
{
var pinchRecognizer = GestureRecognizerCollection.shared.GetPinchGestureRecognizer();
if (pinchRecognizer.State != GestureRecognizerState.Ended)
{
return;
}
if (!InputRegistry.shared.MayHandle(InputType.Click | InputType.Touch, this))
{
return;
}
bool isPointerOverUI = false;
isPointerOverUI |= EventSystem.current.IsPointerOverGameObject();
if (Input.touchCount > 0)
{
isPointerOverUI |= InputUtils.IsTouchOverUI(Input.GetTouch(0).fingerId);
}
var clickWorldPos = Camera.main.ScreenToWorldPoint(Input.mousePosition);
clickWorldPos.z = 0;
// Snap to grid for joints and movement
if (grid.gameObject.activeSelf && (selectedTool == Tool.Joint || selectedTool == Tool.Move))
{
clickWorldPos = grid.ClosestPointOnGrid(clickWorldPos);
}
// Mouse Down
if (Input.GetMouseButtonDown(0))
{
if (isPointerOverUI)
{
return;
}
switch (selectedTool)
{
case Tool.Bone:
var joint = selectionManager.GetSingleSelected <Joint>();
if (joint != null)
{
creatureBuilder.TryStartingBone(joint);
}
break;
case Tool.Muscle:
var bone = selectionManager.GetSingleSelected <Bone>();
if (bone != null)
{
creatureBuilder.TryStartingMuscle(bone);
}
break;
case Tool.Move:
selectionManager.AddCurrentHoveringToSelection();
jointsToMove = selectionManager.GetJointsToMoveFromSelection();
lastDragPosition = clickWorldPos;
if (grid.gameObject.activeSelf && jointsToMove.Count > 0)
{
// Snap the closest joint to the grid
Joint closestJoint = null;
float closestDistance = float.MaxValue;
foreach (var jointToMove in jointsToMove)
{
var distance = Vector3.Distance(jointToMove.center, clickWorldPos);
if (distance < closestDistance)
{
closestJoint = jointToMove;
closestDistance = distance;
}
}
lastDragPosition = closestJoint.center + (clickWorldPos - grid.ClosestPointOnGrid(closestJoint.center));
}
break;
case Tool.Select:
selectionManager.DeselectAll();
selectionManager.StartSelection(clickWorldPos);
break;
default: break;
}
viewController.Refresh();
}
// Mouse Move
else if (InputUtils.MouseHeld())
{
switch (selectedTool)
{
case Tool.Bone:
var hoveringJoint = selectionManager.GetSingleSelected <Joint>();
creatureBuilder.UpdateCurrentBoneEnd(clickWorldPos, hoveringJoint);
break;
case Tool.Muscle:
var hoveringBone = selectionManager.GetSingleSelected <Bone>();
creatureBuilder.UpdateCurrentMuscleEnd(clickWorldPos, hoveringBone);
break;
case Tool.Move:
if (jointsToMove.Count > 0)
{
creatureBuilder.MoveSelection(jointsToMove, clickWorldPos - lastDragPosition);
lastDragPosition = clickWorldPos;
}
break;
case Tool.Select:
if (isPointerOverUI)
{
break;
}
selectionManager.UpdateSelection(clickWorldPos); break;
default: break;
}
}
// Mouse Up
else if (InputUtils.MouseUp())
{
var creatureEdited = false;
var oldDesign = creatureBuilder.GetDesign();
switch (selectedTool)
{
case Tool.Joint:
if (isPointerOverUI)
{
return;
}
if (Input.touchCount > 1)
{
return;
}
#if UNITY_IOS || UNITY_ANDROID
if (Input.touchCount == 0)
{
return;
}
#endif
creatureEdited = creatureBuilder.TryPlacingJoint(clickWorldPos);
break;
case Tool.Bone:
var hoveringJoint = selectionManager.GetSingleSelected <Joint>();
if (hoveringJoint != null)
{
creatureEdited = creatureBuilder.PlaceCurrentBone();
}
else
{
creatureEdited = false;
creatureBuilder.CancelCurrentBone();
}
break;
case Tool.Muscle:
creatureEdited = creatureBuilder.PlaceCurrentMuscle();
break;
case Tool.Move:
creatureEdited = creatureBuilder.MoveEnded(jointsToMove);
jointsToMove.Clear();
if (!isPointerOverUI)
{
selectionManager.DeselectAll();
}
break;
case Tool.Delete:
if (isPointerOverUI)
{
return;
}
selectionManager.AddCurrentHoveringToSelection();
var selection = selectionManager.GetSelection();
creatureEdited = selection.Count > 0;
creatureBuilder.Delete(selection);
selectionManager.DeselectAll();
break;
case Tool.Select:
selectionManager.EndSelection();
OnSelectionEnded();
break;
default: break;
}
if (creatureEdited)
{
historyManager.Push(oldDesign);
if (selectedTool == Tool.Move)
{
creatureBuilder.RefreshMuscleColliders();
}
}
viewController.Refresh();
}
}
void bfReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
// The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
// As long as those body objects are not disposed and not set to null in the array,
// those body objects will be re-used.
bodyFrame.GetAndRefreshBodyData(this.bodies);
}
}
if (this.bodies != null && this.bodies.Length > 0)
{
foreach (Body body in this.bodies)
{
if (body.IsTracked)
{
lblLefthand.Text = "Left Hand : " + GetHandStateString(body.HandLeftState);
lblRighthand.Text = "Right Hand : " + GetHandStateString(body.HandRightState);
//IReadOnlyDictionary<JointType, Joint> joints = body.Joints;
//// convert the joint points to depth (display) space
//Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
//foreach (JointType jointType in joints.Keys)
//{
// // sometimes the depth(Z) of an inferred joint may show as negative
// // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
// CameraSpacePoint position = joints[jointType].Position;
// if (position.Z < 0)
// {
// position.Z = InferredZPositionClamp;
// }
// DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
// jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
//}
Joint rightThumb = body.Joints[JointType.HandTipRight];
Joint rightWrist = body.Joints[JointType.WristRight];
Joint rightShoulder = body.Joints[JointType.ShoulderRight];
Joint rightTb = body.Joints[JointType.ThumbRight];
Joint leftTip = body.Joints[JointType.HandTipLeft];
Joint leftShoulder = body.Joints[JointType.ShoulderLeft];
CameraSpacePoint cspt = rightThumb.Position;
CameraSpacePoint csptW = rightWrist.Position;
CameraSpacePoint csptS = rightShoulder.Position;
CameraSpacePoint csptT = rightTb.Position;
CameraSpacePoint csptLTip = leftTip.Position;
CameraSpacePoint csptLShoulder = leftShoulder.Position;
string rightThumbPosition = "\n Right Thumb : ";
if (rightThumb != null && rightThumb.TrackingState != TrackingState.NotTracked)
{
rightThumbPosition += " tracked/infered.. ";
if (cspt.Z < 0)
{
cspt.Z = 0.1f; //to avoid mapping errors as it may return -ve value for Z some times
}
DepthSpacePoint dspt = mapper.MapCameraPointToDepthSpace(cspt);
DepthSpacePoint dsptW = mapper.MapCameraPointToDepthSpace(csptW);
DepthSpacePoint dsptS = mapper.MapCameraPointToDepthSpace(csptS);
DepthSpacePoint dsptT = mapper.MapCameraPointToDepthSpace(csptT);
DepthSpacePoint dsptLTip = mapper.MapCameraPointToDepthSpace(csptLTip);
DepthSpacePoint dsptLShoulder = mapper.MapCameraPointToDepthSpace(csptLShoulder);
rightThumbPosition += "Depth space -- X : " + dspt.X + " Y : " + dspt.Y + "\n";
// rightThumbPosition += "Depth spaceW -- X : " + dsptW.X + " Y : " + dsptW.Y + "\n";
//srightThumbPosition += "Depth spaceS -- Y : " + dsptS.Y ;
lock (this)
{
if (DoAutoHelp && !IsProcessing && !CanRecord)
{
IsProcessing = true;
CheckAutoHelpCoordinates(dspt, dsptLTip);
IsProcessing = false;
}
}
switch (body.HandLeftState)
{
case HandState.Open:
break;
case HandState.Closed:
break;
case HandState.Lasso:
lock (this)
{
if (CanRecord && !IsProcessing && dsptLShoulder.Y > dsptLTip.Y)
{
IsProcessing = true;
RecordCoordinates(dspt, dsptW, dsptS, dsptT);
IsProcessing = false;
}
}
break;
}
}
else
{
rightThumbPosition += "NOT tracked.. ";
}
lblRighthand.Text += rightThumbPosition;
lblStatus.Text = "Bodies are being tracked...";
switch (body.HandRightState)
{
case HandState.Open:
break;
case HandState.Closed:
break;
case HandState.Lasso:
break;
}
}
}
}
else
{
lblStatus.Text = "No bodies to track..";
}
}
private void dataGridView_CellValueChanged(object sender, DataGridViewCellEventArgs e)
{
if (e.RowIndex > -1)
{
viewportLayout1.Clear();
//viewportLayout1.Labels.Clear();
Dictionary <string, Dictionary <string, double> > panel_data = new Dictionary <string, Dictionary <string, double> >();
Dictionary <string, Dictionary <int, Dictionary <string, double> > > cell_data_by_axis = new Dictionary <string, Dictionary <int, Dictionary <string, double> > >();
Dictionary <int, Dictionary <string, Dictionary <string, double> > > cell_data_by_id = new Dictionary <int, Dictionary <string, Dictionary <string, double> > >();
panel_data.Add("X", new Dictionary <string, double>());
panel_data.Add("Y", new Dictionary <string, double>());
panel_data.Add("Z", new Dictionary <string, double>());
cell_data_by_axis.Add("X", new Dictionary <int, Dictionary <string, double> >());
cell_data_by_axis.Add("Y", new Dictionary <int, Dictionary <string, double> >());
cell_data_by_axis.Add("Z", new Dictionary <int, Dictionary <string, double> >());
panel_data["X"].Add("Average", 0);
panel_data["Y"].Add("Average", 0);
panel_data["Z"].Add("Average", 0);
panel_data["X"].Add("Row Count", 0);
panel_data["Y"].Add("Row Count", 0);
panel_data["Z"].Add("Row Count", 0);
panel_data["X"].Add("Max", 0);
panel_data["Y"].Add("Max", 0);
panel_data["Z"].Add("Max", 0);
panel_data["X"].Add("Min", 0);
panel_data["Y"].Add("Min", 0);
panel_data["Z"].Add("Min", 0);
panel_data["X"].Add("Variance", 0);
panel_data["Y"].Add("Variance", 0);
panel_data["Z"].Add("Variance", 0);
panel_data["X"].Add("DeltaMax", 0);
panel_data["Y"].Add("DeltaMax", 0);
panel_data["Z"].Add("DeltaMax", 0);
panel_data["X"].Add("DeltaAvg", 0);
panel_data["Y"].Add("DeltaAvg", 0);
panel_data["Z"].Add("DeltaAvg", 0);
panel_data["X"].Add("Median", 0);
panel_data["Y"].Add("Median", 0);
panel_data["Z"].Add("Median", 0);
//Pass valid data from the grid to the dictionary
foreach (DataGridViewRow row in this.dataGridView.Rows)
{
if (row.Cells[0].Value != null && Information.IsNumeric(row.Cells[0].Value) &&
row.Cells[1].Value != null &&
row.Cells[2].Value != null && Information.IsNumeric(row.Cells[2].Value) &&
row.Cells[3].Value != null && Information.IsNumeric(row.Cells[3].Value) &&
row.Cells[4].Value != null && Information.IsNumeric(row.Cells[4].Value) &&
row.Cells[5].Value != null && Information.IsNumeric(row.Cells[5].Value)
)
{
var id = int.Parse(row.Cells[0].Value.ToString());
var axis = row.Cells[1].Value.ToString();
var value = double.Parse(row.Cells[2].Value.ToString());
var nominal = double.Parse(row.Cells[3].Value.ToString());
var lowTolerance = double.Parse(row.Cells[4].Value.ToString());
var upTolerance = double.Parse(row.Cells[5].Value.ToString());
if (cell_data_by_axis[axis].ContainsKey(id))
{
MessageBox.Show("Duplicate measurements with the id " + id.ToString() + " on the " + axis.ToString() + " axis.");
}
else
{
cell_data_by_axis[axis].Add(
id,
new Dictionary <string, double>()
);
cell_data_by_axis[axis][id].Add("Value", value);
cell_data_by_axis[axis][id].Add("Nominal", nominal);
cell_data_by_axis[axis][id].Add("LowTolerance", lowTolerance);
cell_data_by_axis[axis][id].Add("UpTolerance", upTolerance);
if (!cell_data_by_id.ContainsKey(id))
{
cell_data_by_id[id] = new Dictionary <string, Dictionary <string, double> >();
cell_data_by_id[id]["X"] = new Dictionary <string, double>();
cell_data_by_id[id]["Y"] = new Dictionary <string, double>();
cell_data_by_id[id]["Z"] = new Dictionary <string, double>();
cell_data_by_id[id]["X"]["Value"] = 0;
cell_data_by_id[id]["Y"]["Value"] = 0;
cell_data_by_id[id]["Z"]["Value"] = 0;
cell_data_by_id[id]["X"]["Nominal"] = 0;
cell_data_by_id[id]["Y"]["Nominal"] = 0;
cell_data_by_id[id]["Z"]["Nominal"] = 0;
cell_data_by_id[id]["X"]["LowTolerance"] = 0;
cell_data_by_id[id]["Y"]["LowTolerance"] = 0;
cell_data_by_id[id]["Z"]["LowTolerance"] = 0;
cell_data_by_id[id]["X"]["UpTolerance"] = 0;
cell_data_by_id[id]["Y"]["UpTolerance"] = 0;
cell_data_by_id[id]["Z"]["UpTolerance"] = 0;
}
cell_data_by_id[id][axis]["Value"] = value;
cell_data_by_id[id][axis]["Nominal"] = nominal;
cell_data_by_id[id][axis]["LowTolerance"] = lowTolerance;
cell_data_by_id[id][axis]["UpTolerance"] = upTolerance;
}
}
}
//Iterate over data dictionary
foreach (var axis in cell_data_by_axis)
{
List <double> median_list = new List <double>();
foreach (var value in axis.Value)
{
panel_data[axis.Key]["Average"] += value.Value["Value"];
median_list.Add(value.Value["Value"]);
//Biggest value
if (value.Value["Value"] > panel_data[axis.Key]["Max"] || panel_data[axis.Key]["Max"] == 0)
{
panel_data[axis.Key]["Max"] = value.Value["Value"];
}
//Smallest value
if (value.Value["Value"] < panel_data[axis.Key]["Min"] || panel_data[axis.Key]["Min"] == 0)
{
panel_data[axis.Key]["Min"] = value.Value["Value"];
}
}
//Average value
panel_data[axis.Key]["Average"] /= axis.Value.Count;
foreach (var value in axis.Value)
{
panel_data[axis.Key]["Variance"] += Math.Pow(value.Value["Value"] - panel_data[axis.Key]["Average"], 2);
}
//Variance value
panel_data[axis.Key]["Variance"] = Math.Sqrt(panel_data[axis.Key]["Variance"] / axis.Value.Count);
//Median value
if (axis.Value.Count > 0)
{
if (axis.Value.Count % 2 == 1)
{
panel_data[axis.Key]["Median"] = median_list.OrderBy(x => x).ToList()[(int)axis.Value.Count / 2];
}
else
{
panel_data[axis.Key]["Median"] = (median_list.OrderBy(x => x).ToList()[(int)median_list.Count / 2] + median_list.OrderBy(x => x).ToList()[(int)median_list.Count / 2 - 1]) / 2;
}
}
var skippedFirstLoop = false;
var first_var = 0.0;
var second_var = 0.0;
foreach (var value in axis.Value)
{
first_var = value.Value["Value"];
if (skippedFirstLoop)
{
if (Information.IsNumeric(value.Value["Value"]))
{
if (Math.Abs(first_var - second_var) > panel_data[axis.Key]["DeltaMax"])
{
//Greatest change
panel_data[axis.Key]["DeltaMax"] = Math.Abs(first_var - second_var);
}
panel_data[axis.Key]["DeltaAvg"] += Math.Abs(first_var - second_var);
}
}
else
{
skippedFirstLoop = true;
}
second_var = value.Value["Value"];
}
//Average change
panel_data[axis.Key]["DeltaAvg"] /= axis.Value.Count;
}
int valid_measurement_count = 0;
int invalid_measurement_count = 0;
//Draw viewport items
foreach (var axis in cell_data_by_id)
{
Boolean is_valid_measurement = true;
Point3D pt1 = new Point3D(axis.Value["X"]["Nominal"], axis.Value["Y"]["Nominal"], axis.Value["Z"]["Nominal"]);
Joint j1 = new Joint(pt1, 0.1, 1);
viewportLayout1.Entities.Add(j1, 0, Color.Black);
Point3D pt2 = new Point3D(axis.Value["X"]["Value"], axis.Value["Y"]["Value"], axis.Value["Z"]["Value"]);
Joint j2 = new Joint(pt2, 0.2, 1);
viewportLayout1.Entities.Add(new Line(pt1, pt2), 0, Color.Black);
var color = Color.Green;
viewportLayout1.Labels.Add(new LeaderAndText(axis.Value["X"]["Value"], axis.Value["Y"]["Value"], axis.Value["Z"]["Value"],
"Measurement " + axis.Key, new Font("Tahoma", 8.25f), Color.Black, new Vector2D(0, 10)));
if (axis.Value["X"]["Value"] > axis.Value["X"]["Nominal"] + axis.Value["X"]["UpTolerance"] ||
axis.Value["X"]["Value"] < axis.Value["X"]["Nominal"] + axis.Value["X"]["LowTolerance"])
{
color = Color.Red;
var label_message = "X-axis out of tolerance\n X: " + axis.Value["X"]["Value"] + " | Valid range: " + (axis.Value["X"]["Nominal"] + axis.Value["X"]["LowTolerance"]).ToString() + " - " + (axis.Value["X"]["Nominal"] + axis.Value["X"]["UpTolerance"]).ToString();
viewportLayout1.Labels.Add(new LeaderAndText(axis.Value["X"]["Value"], axis.Value["Y"]["Value"], axis.Value["Z"]["Value"],
label_message, new Font("Tahoma", 8.25f), Color.Black, new Vector2D(0, 25)));
is_valid_measurement = false;
}
if (axis.Value["Y"]["Value"] > axis.Value["Y"]["Nominal"] + axis.Value["Y"]["UpTolerance"] ||
axis.Value["Y"]["Value"] < axis.Value["Y"]["Nominal"] + axis.Value["Y"]["LowTolerance"])
{
color = Color.Red;
var label_message = "Y-axis out of tolerance\n Y: " + axis.Value["Y"]["Value"] + " | Valid range: " + (axis.Value["Y"]["Nominal"] + axis.Value["Y"]["LowTolerance"]).ToString() + " - " + (axis.Value["Y"]["Nominal"] + axis.Value["Y"]["UpTolerance"]).ToString();
viewportLayout1.Labels.Add(new LeaderAndText(axis.Value["X"]["Value"], axis.Value["Y"]["Value"], axis.Value["Z"]["Value"],
label_message, new Font("Tahoma", 8.25f), Color.Black, new Vector2D(0, 50)));
is_valid_measurement = false;
}
if (axis.Value["Z"]["Value"] > axis.Value["Z"]["Nominal"] + axis.Value["Z"]["UpTolerance"] ||
axis.Value["Z"]["Value"] < axis.Value["Z"]["Nominal"] + axis.Value["Z"]["LowTolerance"])
{
color = Color.Red;
var label_message = "Z-axis out of tolerance\n Z: " + axis.Value["Z"]["Value"] + " | Valid range: " + (axis.Value["Z"]["Nominal"] + axis.Value["Z"]["LowTolerance"]).ToString() + " - " + (axis.Value["Z"]["Nominal"] + axis.Value["Z"]["UpTolerance"]).ToString();
viewportLayout1.Labels.Add(new LeaderAndText(axis.Value["X"]["Value"], axis.Value["Y"]["Value"], axis.Value["Z"]["Value"],
label_message, new Font("Tahoma", 8.25f), Color.Black, new Vector2D(0, 75)));
is_valid_measurement = false;
}
if (is_valid_measurement)
{
valid_measurement_count++;
}
else
{
invalid_measurement_count++;
}
viewportLayout1.Entities.Add(j2, 0, color);
}
//Setup labels
this.average_x_label_value.Text = (panel_data["X"]["Average"]).ToString();
this.average_y_label_value.Text = (panel_data["Y"]["Average"]).ToString();
this.average_z_label_value.Text = (panel_data["Z"]["Average"]).ToString();
this.max_x_label_value.Text = (panel_data["X"]["Max"]).ToString();
this.max_y_label_value.Text = (panel_data["Y"]["Max"]).ToString();
this.max_z_label_value.Text = (panel_data["Z"]["Max"]).ToString();
this.min_x_label_value.Text = (panel_data["X"]["Min"]).ToString();
this.min_y_label_value.Text = (panel_data["Y"]["Min"]).ToString();
this.min_z_label_value.Text = (panel_data["Z"]["Min"]).ToString();
this.variance_x_label_value.Text = (panel_data["X"]["Variance"]).ToString();
this.variance_y_label_value.Text = (panel_data["Y"]["Variance"]).ToString();
this.variance_z_label_value.Text = (panel_data["Z"]["Variance"]).ToString();
this.deltaMax_x_label_value.Text = (panel_data["X"]["DeltaMax"]).ToString();
this.deltaMax_y_label_value.Text = (panel_data["Y"]["DeltaMax"]).ToString();
this.deltaMax_z_label_value.Text = (panel_data["Z"]["DeltaMax"]).ToString();
this.deltaAvg_x_label_value.Text = (panel_data["X"]["DeltaAvg"]).ToString();
this.deltaAvg_y_label_value.Text = (panel_data["Y"]["DeltaAvg"]).ToString();
this.deltaAvg_z_label_value.Text = (panel_data["Z"]["DeltaAvg"]).ToString();
this.median_x_label_value.Text = (panel_data["X"]["Median"]).ToString();
this.median_y_label_value.Text = (panel_data["Y"]["Median"]).ToString();
this.median_z_label_value.Text = (panel_data["Z"]["Median"]).ToString();
this.valid_measure_count_label.Text = valid_measurement_count.ToString() + " out of " + cell_data_by_id.Count.ToString();
this.invalid_measure_count_label.Text = invalid_measurement_count.ToString() + " out of " + cell_data_by_id.Count.ToString();
viewportLayout1.ZoomFit(100);
viewportLayout1.Refresh();
}
}
/// <summary>
/// Update the filter for one joint.
/// </summary>
/// <param name="skeleton">The Skeleton to filter.</param>
/// <param name="jt">The Skeleton Joint index to filter.</param>
/// <param name="smoothingParameters">The Smoothing parameters to apply.</param>
protected void FilterJoint(Skeleton skeleton, JointType jt, TransformSmoothParameters smoothingParameters)
{
if (null == skeleton)
{
return;
}
int jointIndex = (int)jt;
Vector3 filteredPosition;
Vector3 diffvec;
Vector3 trend;
float diffVal;
Vector3 rawPosition = KinectHelper.SkeletonPointToVector3(skeleton.Joints[jt].Position);
Vector3 prevFilteredPosition = this.history[jointIndex].FilteredPosition;
Vector3 prevTrend = this.history[jointIndex].Trend;
Vector3 prevRawPosition = this.history[jointIndex].RawPosition;
bool jointIsValid = KinectHelper.JointPositionIsValid(rawPosition);
// If joint is invalid, reset the filter
if (!jointIsValid)
{
this.history[jointIndex].FrameCount = 0;
}
// Initial start values
if (this.history[jointIndex].FrameCount == 0)
{
filteredPosition = rawPosition;
trend = Vector3.Zero;
}
else if (this.history[jointIndex].FrameCount == 1)
{
filteredPosition = Vector3.Multiply(Vector3.Add(rawPosition, prevRawPosition), 0.5f);
diffvec = Vector3.Subtract(filteredPosition, prevFilteredPosition);
trend = Vector3.Add(Vector3.Multiply(diffvec, smoothingParameters.Correction), Vector3.Multiply(prevTrend, 1.0f - smoothingParameters.Correction));
}
else
{
// First apply jitter filter
diffvec = Vector3.Subtract(rawPosition, prevFilteredPosition);
diffVal = Math.Abs(diffvec.Length());
if (diffVal <= smoothingParameters.JitterRadius)
{
filteredPosition = Vector3.Add(Vector3.Multiply(rawPosition, diffVal / smoothingParameters.JitterRadius), Vector3.Multiply(prevFilteredPosition, 1.0f - (diffVal / smoothingParameters.JitterRadius)));
}
else
{
filteredPosition = rawPosition;
}
// Now the double exponential smoothing filter
filteredPosition = Vector3.Add(Vector3.Multiply(filteredPosition, 1.0f - smoothingParameters.Smoothing), Vector3.Multiply(Vector3.Add(prevFilteredPosition, prevTrend), smoothingParameters.Smoothing));
diffvec = Vector3.Subtract(filteredPosition, prevFilteredPosition);
trend = Vector3.Add(Vector3.Multiply(diffvec, smoothingParameters.Correction), Vector3.Multiply(prevTrend, 1.0f - smoothingParameters.Correction));
}
// Predict into the future to reduce latency
Vector3 predictedPosition = Vector3.Add(filteredPosition, Vector3.Multiply(trend, smoothingParameters.Prediction));
// Check that we are not too far away from raw data
diffvec = Vector3.Subtract(predictedPosition, rawPosition);
diffVal = Math.Abs(diffvec.Length());
if (diffVal > smoothingParameters.MaxDeviationRadius)
{
predictedPosition = Vector3.Add(Vector3.Multiply(predictedPosition, smoothingParameters.MaxDeviationRadius / diffVal), Vector3.Multiply(rawPosition, 1.0f - (smoothingParameters.MaxDeviationRadius / diffVal)));
}
// Save the data from this frame
this.history[jointIndex].RawPosition = rawPosition;
this.history[jointIndex].FilteredPosition = filteredPosition;
this.history[jointIndex].Trend = trend;
this.history[jointIndex].FrameCount++;
// Set the filtered data back into the joint
Joint j = skeleton.Joints[jt];
j.Position = KinectHelper.Vector3ToSkeletonPoint(predictedPosition);
skeleton.Joints[jt] = j;
}
public static GearJoint createGearJoint(World world, Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio)
{
return(FarseerPhysics.Factories.JointFactory.CreateGearJoint(world, bodyA, bodyB, jointA, jointB, ratio));
}
public Segment(Joint left, Joint right)
{
this.left = left;
this.right = right;
distance = CalculateDistance();
}
public void ConstructJointAllProperties()
{
Axis axis = new Axis(new XyzAttribute(1, 2, 3));
Calibration calibration = new Calibration(1, 2);
Dynamics dynamics = new Dynamics(1, 2);
Limit limit = new Limit(1, 2);
Mimic mimic = new Mimic(new Joint.Builder("mimic", TEST_JOINT_TYPE, TEST_PARENT_LINK, TEST_CHILD_LINK).Build());
Origin origin = new Origin.Builder().SetXyz(new XyzAttribute(1, 2, 3)).SetRpy(new RpyAttribute(1, 2, 3)).Build();
SafetyController safetyController = new SafetyController(1);
Joint.Builder builder = new Joint.Builder(TEST_JOINT_NAME, TEST_JOINT_TYPE, TEST_PARENT_LINK, TEST_CHILD_LINK);
builder.SetAxis(axis);
builder.SetCalibration(calibration);
builder.SetDynamics(dynamics);
builder.SetLimit(limit);
builder.SetMimic(mimic);
builder.SetOrigin(origin);
builder.SetSafetyController(safetyController);
Joint joint = builder.Build();
Assert.AreEqual(TEST_JOINT_NAME, joint.Name);
Assert.AreEqual(TEST_JOINT_TYPE, joint.Type);
Assert.AreEqual(TEST_PARENT_LINK, joint.Parent);
Assert.AreEqual(TEST_CHILD_LINK, joint.Child);
Assert.AreEqual(axis, joint.Axis);
Assert.AreEqual(axis.Xyz.X, joint.Axis.Xyz.X);
Assert.AreEqual(axis.Xyz.Y, joint.Axis.Xyz.Y);
Assert.AreEqual(axis.Xyz.Z, joint.Axis.Xyz.Z);
Assert.AreEqual(calibration, joint.Calibration);
Assert.AreEqual(calibration.Rising, joint.Calibration.Rising);
Assert.AreEqual(calibration.Falling, joint.Calibration.Falling);
Assert.AreEqual(dynamics, joint.Dynamics);
Assert.AreEqual(dynamics.Damping, joint.Dynamics.Damping);
Assert.AreEqual(dynamics.Friction, joint.Dynamics.Friction);
Assert.AreEqual(limit, joint.Limit);
Assert.AreEqual(limit.Lower, joint.Limit.Lower);
Assert.AreEqual(limit.Upper, joint.Limit.Upper);
Assert.AreEqual(limit.Effort, joint.Limit.Effort);
Assert.AreEqual(limit.Velocity, joint.Limit.Velocity);
Assert.AreEqual(mimic, joint.Mimic);
Assert.AreEqual(mimic.Joint, joint.Mimic.Joint);
Assert.AreEqual(mimic.Multiplier, joint.Mimic.Multiplier);
Assert.AreEqual(mimic.Offset, joint.Mimic.Offset);
Assert.AreEqual(origin, joint.Origin);
Assert.AreEqual(origin.Xyz.X, joint.Origin.Xyz.X);
Assert.AreEqual(origin.Xyz.Y, joint.Origin.Xyz.Y);
Assert.AreEqual(origin.Xyz.Z, joint.Origin.Xyz.Z);
Assert.AreEqual(origin.Rpy.R, joint.Origin.Rpy.R);
Assert.AreEqual(origin.Rpy.P, joint.Origin.Rpy.P);
Assert.AreEqual(origin.Rpy.Y, joint.Origin.Rpy.Y);
Assert.AreEqual(safetyController, joint.SafetyController);
Assert.AreEqual(safetyController.SoftLowerLimit, joint.SafetyController.SoftLowerLimit);
Assert.AreEqual(safetyController.SoftUpperLimit, joint.SafetyController.SoftUpperLimit);
Assert.AreEqual(safetyController.KPosition, joint.SafetyController.KPosition);
Assert.AreEqual(safetyController.KVelocity, joint.SafetyController.KVelocity);
}
public void Add(Joint joint)
{
Debug.Assert(JointCount < JointCapacity);
_joints[JointCount++] = joint;
}
/// <summary>
/// Returns the length of the segment defined by the specified joints.
/// </summary>
/// <param name="p1">The first joint (start of the segment).</param>
/// <param name="p2">The second joint (end of the segment).</param>
/// <returns>The length of the segment in meters.</returns>
public static double Distance(Joint p1, Joint p2)
{
return(CameraExtensions.Distance(p1.Position, p2.Position));
}
