public void CalculateQuadrant()
{
var firstQuadrant = new Point {
X = 3, Y = 9
};
var secondQuadrant = new Point {
X = -3, Y = 9
};
var thirdQuadrant = new Point {
X = -3, Y = -9
};
var fourthQuadrant = new Point {
X = 3, Y = -9
};
var fuzzyHelper = new FuzzyHelper();
var quadrantI = fuzzyHelper.InWhichQuadrant(firstQuadrant);
var quadrantII = fuzzyHelper.InWhichQuadrant(secondQuadrant);
var quadrantIII = fuzzyHelper.InWhichQuadrant(thirdQuadrant);
var quadrantIV = fuzzyHelper.InWhichQuadrant(fourthQuadrant);
quadrantI.ShouldBe(Quadrant.I);
quadrantII.ShouldBe(Quadrant.II);
quadrantIII.ShouldBe(Quadrant.III);
quadrantIV.ShouldBe(Quadrant.IV);
}
private double FindTheta1WhenJointPointGiven(Point jointPoint)
{
var vectorX = new Point {
X = 100, Y = 0
};
var angle = _fuzzyHelper.AngleBetweenVectors(jointPoint, vectorX).Round();
var quadrant = _fuzzyHelper.InWhichQuadrant(jointPoint);
switch (quadrant)
{
//case Quadrant.II:
// return (Math.PI - angle).Round();
case Quadrant.III:
case Quadrant.IV:
return((2 * Math.PI - angle).Round());
case Quadrant.None:
case Quadrant.I:
default:
return(angle);
}
}
public void CalculateAngle()
{
var vectorA = new Point()
{
X = 100, Y = 0
};
var vectorB = new Point()
{
X = -20, Y = 20
};
var fuzzyHelper = new FuzzyHelper();
var angle = fuzzyHelper.AngleBetweenVectors(vectorA, vectorB).ConvertRadiansToDegrees();
angle.ShouldBe(135, 0.5);
vectorA = new Point()
{
X = 100, Y = 0
};
vectorB = new Point()
{
X = 20, Y = -20
};
angle = fuzzyHelper.AngleBetweenVectors(vectorA, vectorB);
var quadrant = fuzzyHelper.InWhichQuadrant(vectorB);
switch (quadrant)
{
case Quadrant.IV:
angle = (2 * Math.PI - angle).Round(); break;
case Quadrant.None:
case Quadrant.I:
default:
break;
}
angle.ConvertRadiansToDegrees().ShouldBe(315, 0.5);
vectorA = new Point()
{
X = 100, Y = 0
};
vectorB = new Point()
{
X = -20, Y = -20
};
angle = fuzzyHelper.AngleBetweenVectors(vectorA, vectorB);
quadrant = fuzzyHelper.InWhichQuadrant(vectorB);
switch (quadrant)
{
case Quadrant.III:
case Quadrant.IV:
angle = (2 * Math.PI - angle).Round();
break;
case Quadrant.None:
case Quadrant.I:
default:
break;
}
angle.ConvertRadiansToDegrees().ShouldBe(225, 0.5);
}
public void CalculateQuadrantByVector(int jointLocation)
{
var jointPosition = new Point();
var firstQuadrant = new Point {
X = 4, Y = 9
};
var secondQuadrant = new Point {
X = 2, Y = 10
};
var thirdQuadrant = new Point {
X = 1, Y = 9
};
var fourthQuadrant = new Point {
X = 4, Y = 6
};
switch (jointLocation)
{
case 1:
jointPosition = new Point {
X = 3, Y = 9
};
firstQuadrant = new Point {
X = 4, Y = 9
};
secondQuadrant = new Point {
X = 2, Y = 10
};
thirdQuadrant = new Point {
X = 1, Y = 9
};
fourthQuadrant = new Point {
X = 4, Y = 6
};
break;
case 2:
jointPosition = new Point {
X = -3, Y = 9
};
firstQuadrant = new Point {
X = -4, Y = 9
};
secondQuadrant = new Point {
X = -2, Y = 10
};
thirdQuadrant = new Point {
X = -1, Y = 9
};
fourthQuadrant = new Point {
X = -4, Y = 6
};
break;
case 3:
jointPosition = new Point {
X = -3, Y = -9
};
firstQuadrant = new Point {
X = -4, Y = -9
};
secondQuadrant = new Point {
X = -2, Y = -10
};
thirdQuadrant = new Point {
X = -1, Y = -9
};
fourthQuadrant = new Point {
X = -4, Y = -6
};
break;
case 4:
jointPosition = new Point {
X = 3, Y = -9
};
firstQuadrant = new Point {
X = 4, Y = -9
};
secondQuadrant = new Point {
X = 2, Y = -10
};
thirdQuadrant = new Point {
X = 1, Y = -9
};
fourthQuadrant = new Point {
X = 4, Y = -6
};
break;
}
var fuzzyHelper = new FuzzyHelper();
var quadrantI = fuzzyHelper.InWhichQuadrant(jointPosition, firstQuadrant);
var quadrantII = fuzzyHelper.InWhichQuadrant(jointPosition, secondQuadrant);
var quadrantIII = fuzzyHelper.InWhichQuadrant(jointPosition, thirdQuadrant);
var quadrantIV = fuzzyHelper.InWhichQuadrant(jointPosition, fourthQuadrant);
quadrantI.ShouldBe(Quadrant.IV);
quadrantII.ShouldBe(Quadrant.I);
quadrantIII.ShouldBe(Quadrant.II);
quadrantIV.ShouldBe(Quadrant.III);
}
