public double calculateAngle(Point3D p)
{
double scalarProduct = this.dotProduct(p);
double magnitudeA = this.magnitude();
double magnitudeB = p.magnitude();
double theta = Math.Acos(scalarProduct / (magnitudeA * magnitudeB)) * (180.0 / Math.PI);
return theta;
}
public VolumeDetector()
{
this._rightHandHistory = new ScenarioStateHistory(30,0);
this._leftHandHistory = new ScenarioStateHistory(30,0);
rh_adjustInitiated = false;
lh_adjustInitiated = false;
rh_start = new Point3D(-1, -1, -1);
lh_start = new Point3D(-1, -1, -1);
rh_last = new Point3D(-1, -1, -1);
lh_last = new Point3D(-1, -1, -1);
rh_deltaY = 0;
lh_deltaY = 0;
}
protected Boolean stillWithinXBounds(double hand_x, Point3D start)
{
//Console.WriteLine("Z Diff: " + Math.Abs(hand_z - start.Z));
return (Math.Abs(hand_x - start.X) < MAX_X_ALLOWANCE);
//return true;
}
public Point3D subtract(Point3D p)
{
return new Point3D(this.X - p.X, this.Y - p.Y, this.Z - p.Z);
}
public double dotProduct(Point3D p)
{
return (this.X*p.X + this.Y*p.Y + this.Z*p.Z);
}
public Point3D add(Point3D p)
{
return new Point3D(this.X + p.X, this.Y + p.Y, this.Z + p.Z);
}
