I'm looking for better way to calculate if one object facing another or one is behind another. So far I've been able to create this but it seems to not working 100% correctly :(
I also think it should be in radians rather then degrees.
public static float GetAngle(float x1, float y1, float x2, float y2)
{
float xDiff = x2 - x1;
float yDiff = y2 - y1;
var angle = RadianToDegree((float)Math.Atan2(yDiff, xDiff));
if (angle < 0)
return 360 + angle;
return angle;
}
public static bool IsFacing(Point obj, float rotationAngle, Point target, float arcDegree = 180)
{
var angleBetweenPoints = GetAngle(obj, target);
return rotationAngle >= angleBetweenPoints - arcDegree / 2 && rotationAngle <= angleBetweenPoints + arcDegree / 2;
}
public static bool IsBehind(Point obj, float rotationAngle, Point target, float arcDegree = 180)
{
var angleBetweenPoints = GetAngle(obj, target);
var backViewAngle = rotationAngle > 180 ? rotationAngle - 180 : 180 - rotationAngle;
return backViewAngle >= angleBetweenPoints - arcDegree / 2 && backViewAngle <= angleBetweenPoints + arcDegree / 2;
}
At first glance it looks like your problem is in the test of rotationAngle vs angleBetweenPoints in the IsFacing() method.
Given:
rotationAngle = 0, angleBetweenPoints = 350, arcDegree = 180
then the test will be:
test = 0 >= (350 - 180 / 2) && 0 <= (350 + 180 / 2)
= 0 >= 260 && 0 <= 440
= false
This will happen fairly often, and is due to the use of the range [0..360] for your angles. While this is useful in some cases, this is not one of those cases. Far more useful here is to get the difference in the heading and target angles in the range [-180..180], then the comparison to arcDegree becomes much simpler.
Try this:
// Adjust a value to fit in the specified range
public static float Clamp(float v, float low, float high)
{
float range = high - low;
float modifier = ((int)((v - low) / range) - (v < low ? 1 : 0)) * range;
v -= modifier;
return v;
}
// Get the angle between a point+heading and another point
// Returns an angle in the range (-180..180) where:
// res < 0 target clockwise from heading
// res == 0 target at heading
// res > 0 target counter-clockwise from heading
static float RelativeAngle(float heading, float x1, float y1, float x2, float y2)
{
// get angle between points in world coordinates
float angle = RadianToDegree((float)Math.Atan2(y2 - y1, x2 - x1));
// adjust to local coordinates and normalize
return Clamp(angle - heading, -180, 180);
}
public static bool IsFacing(Point obj, float rotationAngle, Point target, float arcDegree = 180)
{
var angleBetweenPoints = RelativeAngle(rotationAngle, obj.x, obj.y, target.x, target.y);
return angleBetweenPoints >= -(arcDegree / 2) && angleBetweenPoints <= (arcDegre / 2);
}
Related
I am trying to do a digging tool for my game, I have x and y coordinates of point A and B, what I want to do is create a curve between these points, nothing graphical I just need loop through the coordinates (float x, float y).
I am not good at explaining so here is a visual example;
The first image is what's happen if I just use a for loop to decrease the y value until middle and then increase it from the middle to end.
//Very specific code for my example
//I wrote it just for this example so I am not sure if it works
float y;
float x;
public void Example(float startX, float endX, float startY, float endY, float depth)
{
y = startY;
x = startX;
float changeAmountOfY = depth / (endX - startX);
for (int i = (int)startX; i < (startX + endX) / 2; i++)
{
x++;
y -= changeAmountOfY;
}
for (int i = (int)(startX + endX) / 2; i < endX; i++)
{
x++;
y += changeAmountOfY;
}
}
public void ChangeCoordinates()
{
Example(100f, 200f, 100f, 100f, 50f);
}
The second image is what I need.
I am developing the game on unity and I am using Vector2 for the coordinates but it is not important.
Pure C# or even C++ is welcome.
It is also fine if someone can just explain the math behind what I am trying to do.
Maybe this can help:
// Calculate radius
int radius = (B.X - A.X) / 2;
// Calculate middle
int middle_x = A.X + radius;
int middle_y = A.Y;
// or
int middle_y = (A.Y + B.Y) / 2;
// Coordinates for a semicircle
// 0 to 180 degree
for (int i = 0; i <= 180; i++)
{
double x_coordinate = middle_x + radius * Math.Cos(i * Math.PI / 180);
// Opened to bottom
double y_coordinate = middle_y + radius * Math.Sin(i * Math.PI / 180);
// or opened to top
double y_coordinate = middle_y - radius * Math.Sin(i * Math.PI / 180);
}
Take a look at unit circle.
I am trying to determine the limits of an arch for a couple of days now without much successes.I know this is more of a geometry type question than programming but here it goes.Let me first show this image to elaborate on what I am trying to achieve in C#.
In the image above you can see a Green Arc, I am trying to determine where the ‘Left Most’ point is on the X axis (and the 'right most' eventually, but for the sake of simplicity I will focus on the ‘left most’ point)
The answer of this image ‘left most’ in the X axis is 5,note: even if the arch has a greater sweep angle the answer is still 5, but if the sweep angle is less than the answer will be more than 5.
I am reading this information from a text based G-code file and here is a sample of the arc drawn in the G-code file.
G1 X10. Y15.
G3 X5. Y10. I10. J10.
The first line indicates the starting-point of the arch, witch is in this case X 10 and Y 15. The second line indicates the end-point, arch-center-point and rotation.
The G3 indicates a Counter-Clockwise rotation of the arch (G2 indicates a Clockwise Rotation).
The X & Y indicates the End-Point and the I & J is the (X & Y respectively) arch-center-point values.The first thing that I have tried is to see if I can detect the arch crosses the 180* red line without much successes mainly because I cannot seem to get angle calculations working correctly when I try to process different types of coordinates.Here is a sample of code I’m unable to finish:
else if (Gval == 3)
{
//Values read from G-Code File
double startX = Convert.ToDouble(oldXval);
double startY = Convert.ToDouble(oldYval);
double endX = Convert.ToDouble(Xval);
double endY = Convert.ToDouble(Yval);
double midX = Convert.ToDouble(Ival);
double midY = Convert.ToDouble(Jval);
//Get Start angle of Line
double startAngle = Math.Atan2(startY - midY, startX - midX);
startAngle = Math.Round(startAngle * (180.0 / Math.PI), 5);//Radiants to Degrees
startAngle = ((startAngle % 360) + 360) % 360;//Normalise
//Get End angle of line
double EndAngle = Math.Atan2(endY - midY, endX - midX);
EndAngle = Math.Round(EndAngle * (180.0 / Math.PI), 5);//Radiants to Degrees
EndAngle = ((EndAngle % 360) + 360) % 360;//Normalise
if (EndAngle == 0) EndAngle = 360;
//Get Raiduis
double raduis = Math.Round((Math.Sqrt(Math.Pow(Math.Abs(startX - midX), 2) + Math.Pow(Math.Abs(startY - midY), 2))),5);
double deltaY = (midY) - (startX);
double deltaX = (midX) - (startY);
double angleInDegrees = Math.Atan(deltaY / deltaX)*180/Math.PI;
if (startAngle <= 180 && EndAngle >= 180)
{
double LeftValue = midX - raduis;
}
}
The code above only works for some specific arcs.
I have Googled around and found mainly topics on Lines intersections and line and circle intersections but only one specifically about lines intersecting arcs, the answer is too vague to make out what is supposed to happen here is the Link
In addition to that there is also a type of arch that I think will have to be processed differently probably by some sort of Pathegorem, but I have not started on how I can do that one. But here is an image of it:
Here is the G-code :
G1 X30. Y15.
G3 X25.4 Y11.96 I30. J10.
And I think the ‘Left most’ X value is 25.4
I would like to know, if you know of a Method or a Library that can help out with this one .Thanks
This should calculate the X and Y bounds of an arc. There are many optimizations that could be made for better performance, but I think this is easier to understand.
class Bounds
{
public double MinX, MinY, MaxX, MaxY;
}
Bounds GetArcBounds(float cx, float cy, float x1, float y1, float x2, float y2)
{
var a1 = GetAngle(y1 - cy, x1 - cx);
var a2 = GetAngle(y2 - cy, x2 - cx);
var r = (float)Math.Sqrt(Math.Pow(y1 - cy, 2) + Math.Pow(x1 - cx, 2));
var bounds = new Bounds();
bounds.MinX = double.MaxValue;
bounds.MinY = double.MaxValue;
bounds.MaxX = double.MinValue;
bounds.MaxY = double.MinValue;
ExpandBounds(bounds, x1, y1);
ExpandBounds(bounds, x2, y2);
if (IsAngleInArc(a1, a2, 0))
ExpandBounds(bounds, cx + r, cy);
if (IsAngleInArc(a1, a2, (float)Math.PI * 0.5f))
ExpandBounds(bounds, cx, cy + r);
if (IsAngleInArc(a1, a2, (float)Math.PI))
ExpandBounds(bounds, cx - r, cy);
if (IsAngleInArc(a1, a2, (float)Math.PI * 1.5f))
ExpandBounds(bounds, cx, cy - r);
return bounds;
}
float GetAngle(float dy, float dx)
{
var a = (float)Math.Atan2(dy, dx);
if (a < 0) a += (float)Math.PI * 2.0f;
return a;
}
void ExpandBounds(Bounds bounds, float x, float y)
{
if (x < bounds.MinX) bounds.MinX = x;
if (y < bounds.MinY) bounds.MinY = y;
if (x > bounds.MaxX) bounds.MaxX = x;
if (y > bounds.MaxY) bounds.MaxY = y;
}
bool IsAngleInArc(float a1, float a2, float test)
{
if (a2 < a1)
{
a2 += (float)Math.PI * 2.0f;
}
if (test < a1)
{
test += (float)Math.PI * 2.0f;
}
return a1 <= test && test <= a2;
}
If you wanted to be able to do both counter-clockwise and clockwise, I believe you could change the logic to the following, although I have not tested this yet.
if (IsAngleInArc(a1, a2, 0) ^ clockwise)
ExpandBounds(bounds, cx + r, cy);
if (IsAngleInArc(a1, a2, (float)Math.PI * 0.5f) ^ clockwise)
ExpandBounds(bounds, cx, cy + r);
if (IsAngleInArc(a1, a2, (float)Math.PI) ^ clockwise)
ExpandBounds(bounds, cx - r, cy);
if (IsAngleInArc(a1, a2, (float)Math.PI * 1.5f) ^ clockwise)
ExpandBounds(bounds, cx, cy - r);
I am trying to extract the axis of rotation and the angle of rotation from a 3x3 rotation
matrix. I am currently using this method but am unsure if I am on track. I am running into issues elsewhere in my program and am thinking this may be a cause but have been unable to verify this myself.
I am working with the XNA framework, if that helps. I looked into using Matrix.Decompose() which returns the scale (vec3) translation(vec3) and rotation(quaternion) but I am not sure that I can extract the data data I need from these values.
public static void MatrixRotationAxisAngle(Matrix m, ref float theta, ref Vector3 rot_axis) {
float trace = m.M11 + m.M22 + m.M33;
float cos_theta = 0.5f * (trace - 1.0f);
if (cos_theta > 0.999999875f) {
//allow some epsilon value
theta = 0.0f;
rot_axis = new Vector3(1.0f, 0.0f, 0.0f);
} else if (cos_theta > -0.999999875f) {
theta = (float)Math.Acos(cos_theta);
rot_axis.X = (m.Up.Z - m.Forward.Y);
rot_axis.Y = (m.Forward.X - m.Right.Z);
rot_axis.Z = (m.Right.Y - m.Up.X);
rot_axis.Normalize();
} else { //angle within PI limits
theta = (float)Math.PI;
//find index of largest diag term in matrix
int index = 0;
if (m.M11 > m.M22 && m.M11 > m.M33) {
index = 0;
}
if (m.M22 > m.M11 && m.M22 > m.M33) {
index = 1;
}
if (m.M33 > m.M11 && m.M33 > m.M22) {
index = 2;
}
switch (index) {
case 0:
float ix = 1.0f / rot_axis.X;
rot_axis.X = (float)Math.Sqrt(m.M11 + 1.0f);
rot_axis.Y = m.M12 * ix;
rot_axis.X = m.M13 * ix;
break;
case 1:
float iy = 1.0f / rot_axis.Y;
rot_axis.Y = (float)Math.Sqrt(m.M22 + 1.0f);
rot_axis.X = m.M21 * iy;
rot_axis.Z = m.M23 * iy;
break;
case 2:
float iz = 1.0f / rot_axis.Y;
rot_axis.Z = (float)Math.Sqrt(m.M33 + 1.0f);
rot_axis.X = m.M31 * iz;
rot_axis.Y = m.M32 * iz;
break;
}
rot_axis.Normalize();
}
}
I suggest to take a look into the pseudocode from this mathematical explanation (pdf), Chapter 2.1 (depending on your matrix). I have successfully used the code in one of my projects, and it returned valid results in my case.
See the matrix in http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations .
As you can see finding theta is pretty easy. Use that in other equations to find the other angles.
I'm working on a project with 3D based objects and manipulating them via my program. I currently have a textbox that allows me to put a heading in degrees and a button that will calculate the required values to make my main object change its heading. This is the code for that function:
private void btnSetHeading_Click(object sender, EventArgs e)
{
if (this.textBoxHeading.Text.Length == 0)
return;
float heading = (float)0;
try
{
heading = float.Parse(this.textBoxHeading.Text);
}
catch (FormatException ex)
{
MessageBox.Show(ex.Message);
return;
}
if (heading < (float)0 || heading > (float)360)
{
MessageBox.Show("Invalid heading parameter. Acceptable range: 0 - 360");
return;
}
float tempCosine = (float)Math.Cos(((heading * Math.PI) / (float)360.0));
float tempSine = -((float)Math.Sin(((heading * Math.PI) / (float)360.0)));
try
{
ProgramInterop.CreateInstance.SetHeading(tempCosine, tempSine);
}
catch (Exception ex)
{
MessageBox.Show("Caught: " + ex.Message);
}
}
If I supply 90 as the heading to face, the results are tempCosine=0.7071068 and tempSine=-0.7071068, which then makes my main object face 90 degrees or due east.
The program requires the heading to be given in two seperate values(tempCosine and tempSine) which I'm not familiar with geometry enough to understand why I would multiply by 360 instead of 180 but this is how its required to work.
Now for the next part of my project involves making my main object face another object given both of their (x,y) coordinates. If for example my main object is at (9112.94, 22088.74) and the new object I want to face is at (9127.04, 22088.88), it would require almost exactly 90 degrees heading to make it face the new object.
How can I calculate the tempCosine and tempSine from those two coordinates?
Regarding 180, that's true. I get used to have an extension class like this for working with radians and degrees.
public static class Extension
{
public static double ToRadians(this double degree)
{
return degree * Math.PI / 180;
}
public static double ToDegrees(this double val)
{
return val * 180 / Math.PI;
}
}
Regarding sine and cosine (I'm not sure I understood evetything right) but if I use the code below
float x1 = 9112.94f;
float y1 = 22088.74f;
float x2 = 9127.04f;
float y2 = 22088.88f;
float r = (float) Math.Pow((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1), 0.5);
float cosine = (x2 - x1) /r;
float sine = (y2 - y1) /r;
I'll get the angle 0.5712978 (not equal to 90).
Sorry if I misunderstood the problem.
I was able to work out the answer using Dani's suggestion to use Atan2(y, x). Thanks Dani.
float x1 = 9112.94f;
float y1 = 22088.74f;
float x2 = 9127.04f;
float y2 = 22088.88f;
float angleRadians;
float diffX = x2 - x1;
float diffY = y2 - y1;
float atan2Result = (float)Math.Atan2(diffX, diffY);
angleRadians = atan2Result / 2;
if (angleRadians < 0.0f)
angleRadians += (float)Math.PI;
float tempCosine = (float)Math.Cos(angleRadians);
float tempSine = -((float)Math.Sin(angleRadians));
I have two functions that are intended to contain angles between (-180,180] and (-π,π]. The intent is that given any angle from -inf to +inf it will retain the equivalent angle in the intervals specified. For example the angle for 1550° is 110°.
public double WrapBetween180(double angle)
{
return angle - 360d * Math.Round(angle / 360d, MidpointRounding.AwayFromZero);
}
public double WrapBetweenPI(double angle)
{
const double twopi = 2d * Math.PI;
return angle - twopi * Math.Round(angle / twopi, MidpointRounding.AwayFromZero);
}
which yields the following results
WrapBetween180(-180) = -180
WrapBetween180( 180) = 180
WrapBetweenPI(-Math.PI) = Math.PI
WrapBetweenPI( Math.PI) = -Math.PI
none of which is what I want. What I wanted is:
WrapBetween180(-180) = 180
WrapBetween180( 180) = 180
WrapBetweenPI(-Math.PI) = Math.PI
WrapBetweenPI( Math.PI) = Math.PI
I tryied playing around with the rounding methods, but still cannot get the desired results. The problem is pronounced because sometimes the angles I deal with are only approximately close to -π or π and I am getting discontinuities it my results.
Any suggestions on how to best implement angle wrapping functions with non-inclusive low limit and inclusive high limits?
For the angle in degrees, if x is between -180 and 180, then 180 - x is between 0 and 360. What you want is equivalent to asking that 180 - x is between 0 (inclusive), and 360 (exclusive). So, as soon as 180 - x reaches 360, we want to add 360 to the angle. This gives us:
return angle + 360d * Math.Floor((180d - angle) / 360d);
Same thing for the angle in radians:
return angle + twopi * Math.Floor((Math.PI - angle) / twopi);
It does not address the rounding issue, but here is how I would to what you want to do :
private static double ConvertAngle(double angle)
{
bool isNegative = angle < 0;
if (isNegative)
angle *= -1;
angle = angle % 360;
if (isNegative)
angle = -1 * angle + 360;
if (angle > 180)
angle = (angle - 360);
return angle;
}
Note: This way supposes you want "behind" to be 180 degrees, not -180 degrees.
Isn't this a case for a modulo operation?
private double Wrap180(double value)
{
// exact rounding of corner values
if (value == 180) return 180.0;
if (value == -180) return 180.0;
// "shift" by 180 and use module, then shift back.
double wrapped = ((Math.Abs(value) + 180.0) % 360.0) - 180.0;
// handle negative values correctly
if (value < 0) return -wrapped;
return wrapped;
}
It passes this tests
Assert.AreEqual(170.0, wrap(-190.0));
Assert.AreEqual(180.0, wrap(-180.0));
Assert.AreEqual(-170.0, wrap(-170.0));
Assert.AreEqual(0.0, wrap(0.0));
Assert.AreEqual(10.0, wrap(10.0));
Assert.AreEqual(170.0, wrap(170.0));
Assert.AreEqual(180.0, wrap(180.0));
Assert.AreEqual(-170.0, wrap(190.0));