public CursorPos CalculateCursorPos(WiimoteState wiimoteState) { int x; int y; IRState irState = wiimoteState.IRState; PointF relativePosition = new PointF(); bool foundMidpoint = false; /*for(int i=0;i<irState.IRSensors.Count() && !foundMidpoint;i++)//IRSensor sensor in irState.IRSensors) { IRSensor sensor = irState.IRSensors[i]; if (sensor.Found) { for (int j = i + 1; j < irState.IRSensors.Count() && !foundMidpoint; j++) { IRSensor sensor2 = irState.IRSensors[j]; if (sensor2.Found) {*/ if(irState.IRSensors[0].Found && irState.IRSensors[1].Found) { foundMidpoint = true; relativePosition.X = (irState.IRSensors[0].Position.X + irState.IRSensors[1].Position.X) / 2.0f; relativePosition.Y = (irState.IRSensors[0].Position.Y + irState.IRSensors[1].Position.Y) / 2.0f; if (Settings.Default.pointer_considerRotation) { //accelSmoothing.addValue(new System.Windows.Vector(args.WiimoteState.AccelState.RawValues.X, args.WiimoteState.AccelState.RawValues.Z)); //System.Windows.Vector smoothedRotation = accelSmoothing.getSmoothedValue(); /* while (accXhistory.Count >= Settings.Default.pointer_rotationSmoothing) { accXhistory.Dequeue(); } while (accZhistory.Count >= Settings.Default.pointer_rotationSmoothing) { accZhistory.Dequeue(); } accXhistory.Enqueue(args.WiimoteState.AccelState.RawValues.X); accZhistory.Enqueue(args.WiimoteState.AccelState.RawValues.Z); smoothedX = 0; smoothedZ = 0; foreach (double accX in accXhistory) { smoothedX += accX; } smoothedX /= accXhistory.Count; foreach (double accZ in accZhistory) { smoothedZ += accZ; } smoothedZ /= accZhistory.Count; */ smoothedX = smoothedX * 0.9 + wiimoteState.AccelState.RawValues.X * 0.1; smoothedZ = smoothedZ * 0.9 + wiimoteState.AccelState.RawValues.Z * 0.1; double absx = Math.Abs(smoothedX - 128), absz = Math.Abs(smoothedZ - 128); if (orientation == 0 || orientation == 2) absx -= 5; if (orientation == 1 || orientation == 3) absz -= 5; if (absz >= absx) { if (absz > 5) orientation = (smoothedZ > 128) ? 0 : 2; } else { if (absx > 5) orientation = (smoothedX > 128) ? 3 : 1; } int l = leftPoint, r; //if (leftPoint == -1) //{ switch (orientation) { case 0: l = (irState.IRSensors[0].RawPosition.X < irState.IRSensors[1].RawPosition.X) ? 0 : 1; break; case 1: l = (irState.IRSensors[0].RawPosition.Y > irState.IRSensors[1].RawPosition.Y) ? 0 : 1; break; case 2: l = (irState.IRSensors[0].RawPosition.X > irState.IRSensors[1].RawPosition.X) ? 0 : 1; break; case 3: l = (irState.IRSensors[0].RawPosition.Y < irState.IRSensors[1].RawPosition.Y) ? 0 : 1; break; } leftPoint = l; //} r = 1 - l; double dx = irState.IRSensors[r].RawPosition.X - irState.IRSensors[l].RawPosition.X; double dy = irState.IRSensors[r].RawPosition.Y - irState.IRSensors[l].RawPosition.Y; double d = Math.Sqrt(dx * dx + dy * dy); dx /= d; dy /= d; smoothedRotation = 0.7 * smoothedRotation + 0.3 * Math.Atan2(dy, dx); /* while (rotationHistory.Count >= Settings.Default.pointer_rotationSmoothing) { rotationHistory.Dequeue(); } rotationHistory.Enqueue(rotation); double smoothedRotation = 0; foreach (double rot in rotationHistory) { smoothedRotation += rot; } smoothedRotation /= rotationHistory.Count; */ //smoothedRotation = smoothedRotation * 0.9 + rotation * 0.1; //rotation = smoothedRotation; } //} //} //} } if (!foundMidpoint) { CursorPos err = lastPos; err.OutOfReach = true; return err; } int offsetY = 0; if (Properties.Settings.Default.pointer_sensorBarPos == "top") { offsetY = -SBPositionOffset; } else if (Properties.Settings.Default.pointer_sensorBarPos == "bottom") { offsetY = SBPositionOffset; } relativePosition.X = 1 - relativePosition.X; if (Settings.Default.pointer_considerRotation) { //accelSmoothing.addValue(new System.Windows.Vector(args.WiimoteState.AccelState.Values.X, args.WiimoteState.AccelState.Values.Z)); //System.Windows.Vector smoothedRotation = accelSmoothing.getSmoothedValue(); //rotation = -1 * (Math.Atan2(smoothedRotation.Y, smoothedRotation.X) - (Math.PI / 2.0)); relativePosition.X = relativePosition.X - 0.5F; relativePosition.Y = relativePosition.Y - 0.5F; relativePosition = this.rotatePoint(relativePosition, smoothedRotation); relativePosition.X = relativePosition.X + 0.5F; relativePosition.Y = relativePosition.Y + 0.5F; //relativePosition.X = 1 - relativePosition.X; //relativePosition.Y = 1 - relativePosition.Y; } x = Convert.ToInt32((float)maxWidth * relativePosition.X + minXPos); y = Convert.ToInt32((float)maxHeight * relativePosition.Y + minYPos) + offsetY; if (x <= 0) { x = 0; } else if (x >= primaryScreen.Bounds.Width) { x = primaryScreen.Bounds.Width - 1; } if (y <= 0) { y = 0; } else if (y >= primaryScreen.Bounds.Height) { y = primaryScreen.Bounds.Height - 1; } CursorPos result = new CursorPos(x, y, relativePosition.X, relativePosition.Y, smoothedRotation); lastPos = result; return result; }
private PointF rotatePoint(PointF point, double angle) { double sin = Math.Sin(angle); double cos = Math.Cos(angle); double xnew = point.X * cos - point.Y * sin; double ynew = point.X * sin + point.Y * cos; PointF result; result.X = (float)xnew; result.Y = (float)ynew; return result; }
public CoordinatesProcessedEventArgs(string ID, float[] AccelState, PointF[] IRState, int battery, PointF MidPoint) { this.ID = ID; AccelCoordinates = AccelState; IRCoordinates = IRState; this.MidPoint = MidPoint; this.battery = battery; }
public PointF CalculateMidPoint() { PointF point = new PointF(); for(int i=0; i < 4; i++) { point.X = point.X + updated_IRState[i].X; point.Y = point.Y + updated_IRState[i].Y; } point.X = point.X / 4; point.Y = point.Y / 4; return point; }
/// <summary> /// Calculates difference between the curent coordinates and the previous coordinates recieved from the remote. /// </summary> /// <param name="e"></param> /// <returns></returns> public PointF Stabilize(WiimoteState e) { PointF AvgDelta; AvgDelta.X = 0; AvgDelta.Y = 0; PointF[] delta = new PointF[4]; int increment = 0; for(int i = 0; i < 4; i++) { if (e.IRState.IRSensors[i].Found) { delta[i].X = e.IRState.IRSensors[i].Position.X - prev_IRState[i].X; delta[i].Y = e.IRState.IRSensors[i].Position.Y - prev_IRState[i].Y; } else { delta[i].X = float.NaN; delta[i].Y = float.NaN; } } foreach (var D in delta) { if (!float.IsNaN(D.X) && !float.IsNaN(D.Y)) { AvgDelta.X += D.X; AvgDelta.Y += D.Y; increment++; } } AvgDelta.X = AvgDelta.X / increment; AvgDelta.Y = AvgDelta.Y / increment; return AvgDelta; }
public bool CheckForArrayNull(PointF[] array) { foreach (var x in array) if (x.X != 0 && x.Y != 0) return false; return true; }