A light source is an entity in 2D space that sits in a single coordinate.
There are multiple light sources around in various locations and each gives off 8 rays of light in directions N, S, E, W, NW, NE, SW, SE. The coordinates of all lights are known.
I need to calculate all intersections of these rays within the grid.
long width = int.MaxValue; // 2D grid width.
long height = int.MaxValue * 3; // 2D grid height.
List<Point> lights = a bunch of randomly placed light sources.
List<Point> intersections = calculate all possible intersections.
for (int i=0; i < lights.Count - 1; i++)
{
for (int j=i + 1; j < lights.Count; j++)
{
// How to compare using integers only?
// If that is not possible, what is the fastest alternative?
}
}
My answer is based off of your comment on a linked question: is there also an easy way to determine at which coordinates diagonal rays of light intersect each other for two given points? It looks like you want to determine the points of the intersection for the rays given by the light sources.
From what you have already described, the horizontal/vertical cases are easy. The points between the two sources describe the intersection. The diagonal cases are more tricky, and I think the easiest way to approach it is just calculating line intersections.
You can describe each diagonal/anti-diagonal as a line described by a vector equation ray = s + u * d where s is the position of the light source and d is the direction of the ray (either [1, 1] ,[1, -1], [1, 0], or [0, 1]). You have four of such equations for each source, one for each direction. Now, to find the intersection of the diagonal, just find the intersection of the non-parallel lines for the two sources (one pair will be parallel, and so cannot intersection).
Sorry if this isn't clear, I'll try to update this.
Update
As a simple optimization, rays intersect diagonally if and only if the rectilinear distance (|x1 - x2| + |y1 - y2|) between the sources is even. I think there's other conditions that help to simplify your case.
Here's a derivation to find the equations you need. We start with two rays:
ray1 = s1 + u1 * d1
ray2 = s2 + u2 * d2
In cartesian coordinates:
ray1x = s1x + u1 * d1x
ray1y = s1y + u1 * d1y
ray2x = s2x + u2 * d2x
ray2y = s2y + u2 * d2y
At the intersection, ray1x = ray2x and ray1y = ray2y:
s1x + u1 * d1x = s2x + u2 * d2x
s1y + u1 * d1y = s2y + u2 * d2y
To make things easier, we can isolate and eliminate u2:
u2 = (s1x - s2x + u1 * d1x) / d2x
u2 = (s1y - s2y + u1 * d1y) / d2y
(s1x - s2x + u1 * d1x) / d2x = (s1y - s2y + u1 * d1y) / d2y
(s1x - s2x + u1 * d1x) * d2y = (s1y - s2y + u1 * d1y) * d2x
Then solve for u1:
(s1x - s2x) * d2y + u1 * d1x * d2y = (s1y - s2y) * d2x + u1 * d1y * d2x
u1 * (d1x * d2y - d1y * d2x) = (s1y - s2y) * d2x - (s1x - s2x) * d2y
u1 = ((s1y - s2y) * d2x - (s1x - s2x) * d2y) / (d1x * d2y - d1y * d2x)
To find u2 you can just evaluate one of equations above or use:
u2 = ((s2y - s1y) * d1x - (s2x - s1x) * d1y) / (d2x * d1y - d2y * d1x)
So there you have it. Two equations to solve for u1 and u2 given the source locations s1, s2 and ray directions d1, d2. You just plug in u1 and u2 values into the original ray equations and you have the intersections for one pair. In your case, an intersection exists if and only if u1 and u2 are integers. There's one case where a division by zero occurs, when the directions are [1, 0] and [0, 1], but that case is trivial to solve (the non-zero coordinates of the sources form the coordinates of the intersection).
Assuming you have a fixed coordinate plane size, and you will be doing these calculation many times for light sources in different positions, you can do better than iterating over every point.
You can create four bool (or bit) Arrays.
Horiz
Verti
DiagR
DiagL
and for each of our light sources, we 'project' them onto those 1-dimensional arrays.
(in the picture I only show two of the projections).
Projecting onto the Horiz and Verti are simple.
Projecting a point (x,y) on the DiagR array shown in the picture is as easy as x plus y.
Now you could simply walk over all of the grid points, and see if at least 2 of its projections are set to true.
But we can do better,
For instance, in the example we can start by walking over the Verti array.
We notice that Verti[0] is set to true, now we want to see if it intersects with Horiz, DiagR, DiagL.
We compute that to check for an intersection with DiagR (the other array in our picture) we only need to see if DiagR[0], DiagR[1], DiagR[2], and DiagR[3] are true, and we can ignore the rest of that array.
The light of Verti[0] can intersect with horiz at any of its elements.
The light of Verti[0] can intersect with DiagL only at DiagL positions 0,1,2, and 3.
Continue for the rest of Verti[i].
We can now do something similar looking for intersections from the true Horiz[i]'s with DiagR and DiagL.
Lastly, we walk over DiagR and look for intersections with DiagL.
This will return you a list of all intersection points of rays, but which also includes the points of the light sources.
You could either just ignore all intersection points that occur where there are point sources, or use some ingenuity to account for those points.
I've lifted the maths from here,
Okay so each point has 4 "cardinal rays", a ray being a infinite line that passes between two points.
// A line in the form Ax+By=C from 2 points
public struct Ray
{
public readonly float A;
public readonly float B;
public readonly float C;
public Ray(PointF one, PointF two)
{
this.A = two.y - one.y;
this.B = one.x - two.x;
this.C = (this.A * one.x) + (this.B * two.x);
}
}
To get the cardinals we can extend PointF
private readonly SizeF NS = new SizeF(0.0F, 1.0F);
private readonly SizeF EW = new SizeF(1.0F, 0.0F);
private readonly SizeF NESW = new SizeF(1.0F, 1.0F);
private readonly SizeF NWSE = new SizeF(-1.0F, 1.0F);
public static IEnumerable<Ray> GetCardinals(this PointF point)
{
yield return new Ray(point + NS, point - NS);
yield return new Ray(point + EW, point - EW);
yield return new Ray(point + NESW, point - NESW);
yield return new Ray(point + NWSE, point - NWSE);
}
To find the inersection of two rays we can do
static PointF Intersection(Ray one, Ray two)
{
var delta = (one.A * two.B) - (two.A * one.B);
if (delta == 0.0F)
{
//Lines are parallel
return PointF.Empty;
}
else
{
var x = ((two.B * one.C) - (one.B * two.C)) / delta;
var y = ((one.A * two.C) - (two.A * one.C)) / delta;
return new PointF(x, y);
}
}
So, to get the intersections for the cardinals of two points,
public static IEnumerable<PointF> GetCardinalIntersections(
this PointF point,
PointF other);
{
return point.GetCardianls().SelectMany(other.GetCardinals(), Intersection)
.Where(i => !i.IsEmpty());
}
Which then enables,
public static IEnumerable<PointF> GetCardinalIntersections(
this PointF point,
IEnumerable<PointF> others);
{
return others.SelectMany((o) => point.GetCardinalIntersections(o));
}
We can then use this functionality like this.
var point = new PointF(1.0F, 1.0F);
var others = new [] { new PointF(2.0F, 5.0F), new PointF(-13.0F, 32.0F) };
var intersections = point.GetCardinalIntersections(others);
Obviously there is lots of iteration here, I haven't compiled or tested this but since, at its nub, the maths seems fairly efficient I'm optimistic about performance.
Related
Is there a nice way to draw an arrowhead, when you have only coordinates of the source and destination point of the line?
So I'm using ITextSharp, programming in c#.
I know how to easily calculate slope and stuff but is there a way of calculating the edge coordinates for the "arrowhead" triangle (marked red in image)?
Thanks in advance
Your central line is vector from (x1,y1) to (x2,y2)
Backward direction vector D = (dx, dy) = (x1-x2, y1-y2)
Length of it: Norm = Sqrt(dx * dx + dy * dy)
Normalize it: uD = (udx, udy) = (dx/Norm, dy/Norm)
To form "wings" of arrow, rotate uD by needed angle. For example, I use angle Pi/6 with Cos(Pi/6) = Sqrt(3)/2 and Sin(Pi/6) = 1/2
ax = udx * Sqrt(3)/2 - udy * 1/2
ay = udx * 1/2 + udy * Sqrt(3)/2
bx = udx * Sqrt(3)/2 + udy * 1/2
by = - udx * 1/2 + udy * Sqrt(3)/2
Points for head with wing length L=20:
(x1 + 20 * ax, y1 + 20 * ay)
and
(x1 + 20 * bx, y1 + 20 * by)
If we define two (2-dimensional) vectors p2 and p3 for the end point of your line and the end point of one side of the arrow respectively you will get p3 by evaluating
p3 = p2 + A(alpha)s
s is a vector (l, 0) where l is the length of one side of the arrow.
alpha is the angle of this line against the x-axis i.e. the sum of the angle of your given line and the positive or negative angle between this line and the arrow line.
The components of the matrix are:
a11 = a22 = cos(alpha)
a12 = -sin(alpha)
a21 = sin(alpha)
Edit: As the y-component of s is zero a12 and a22 are not needed.
I've been looking for a solution to this for some time now and already have many elements to work with but not really how to piece them together.
Objective: Draw a trail for the player's ship.
So far: Since the ship's direction is unpredictable I have only the previous positions of the player's ship to work with. To draw the trail I could simply draw a pixel (or a texture) at the previous position of the player but this is memory expensive and it doesn't draw curves, it won't achieve a pleasing to the eye curved effect.
I've been looking into Beziers Paths and Cathmull Rom for solutions.
Now I can get the control points for a given point, then from 2 points and 2 control points calculate a curve, from here I make an array of VertexPositionColor with a distance between points to make a triangleStrip from the curve.
These are the main functions I have so far:
public Vector2[] GetControlPoints(Vector2 p0, Vector2 p1, Vector2 p2, float tension = 0.5f)
{
// get length of lines [p0-p1] and [p1-p2]
float d01 = Vector2.Distance(p0, p1);
float d12 = Vector2.Distance(p1, p2);
// calculate scaling factors as fractions of total
float sa = tension * d01 / (d01 + d12);
float sb = tension * d12 / (d01 + d12);
// left control point
float c1x = p1.X - sa * (p2.X - p0.X);
float c1y = p1.Y - sa * (p2.Y - p0.Y);
// right control point
float c2x = p1.X + sb * (p2.X - p0.X);
float c2y = p1.Y + sb * (p2.Y - p0.Y);
return new Vector2[] {new Vector2(c1x, c1y), new Vector2(c2x, c2y) };
}
// Given 2 points and 2 control points
public static VertexPositionColor[] bezierCurve(Vector2 start, Vector2 end, Vector2 c1, Vector2 c2)
{
VertexPositionColor[] points = new VertexPositionColor[SUBDIVISIONS + 2];
float fraction;
for (int i = 0; i < SUBDIVISIONS + 2; i++)
{
fraction = i * (1f / (float)SUBDIVISIONS);
points[i] = new VertexPositionColor(new Vector3((float)((start.X * Math.Pow((1 - fraction), 3))
+(c1.X * 3 * fraction * Math.Pow(1-fraction, 2))
+(c2.X * 3 * Math.Pow(fraction,2) * (1-fraction))
+(end.X * Math.Pow(fraction,3))),
(float)((start.Y * Math.Pow((1 - fraction), 3))
+ (c1.Y * 3 * fraction * Math.Pow(1 - fraction, 2))
+ (c2.Y * 3 * Math.Pow(fraction, 2) * (1 - fraction))
+ (end.Y * Math.Pow(fraction, 3))), 0), UNLIT);
}
return points;
}
/*
* This function treats the curve as a series of straight lines and calculates points on a line perpendicular to each point, resulting in two points THICKNESS appart.
* Requires THICKNESS to be set
*/
public static VertexPositionColor[] curveToStrip(VertexPositionColor[] curve)
{
VertexPositionColor[] strip = new VertexPositionColor[curve.Length * 2];
VertexPositionColor[] new1 = new VertexPositionColor[curve.Length];
VertexPositionColor[] new2 = new VertexPositionColor[curve.Length];
for (int i = 0; i < curve.Length; i++)
{
if (i < curve.Length-1)
{
Vector2 p1 = new Vector2(curve[i].Position.X, curve[i].Position.Y);
Vector2 p2 = new Vector2(curve[i + 1].Position.X, curve[i + 1].Position.Y);
Vector2 perpPoint = perpendicularPoint(p1, p2);
new1[i] = new VertexPositionColor(new Vector3(distanceToPoint(p1, perpPoint, THICKNESS / 2), 0), UNLIT);
new2[i] = new VertexPositionColor(new Vector3(distanceToPoint(p1, perpPoint, -1 * THICKNESS / 2), 0), UNLIT);
}
else
{
Vector2 p1 = new Vector2(curve[i].Position.X, curve[i].Position.Y);
Vector2 p2 = new Vector2(curve[i - 1].Position.X, curve[i - 1].Position.Y);
Vector2 perpPoint = perpendicularPoint(p1, p2);
new1[i] = new VertexPositionColor(new Vector3(distanceToPoint(p1, perpPoint, -1 * THICKNESS / 2), 0), UNLIT);
new2[i] = new VertexPositionColor(new Vector3(distanceToPoint(p1, perpPoint, THICKNESS / 2), 0), UNLIT);
}
}
I thought about calling the functions on the draw phase but this seems very expensive just to make a tiny curve and to draw a bigger Beziers path I imagine it worse. Since I would get a point at each frame, each function would be called to calculate the curve between points just to draw 1 curve of 3 pixels (or less).
How can I proceed? Any suggestions?
I am still a beginner on this kind of stuff!
All this I got from several sources:
CathmullRom
Beziers and Triangle strip
http://www.imagehosting.cz/images/trails.gif
I will just briefly explain how this works:
It is function that receives position, it is called each time you want add next segment of trail.
When function is called it adds two vertices on position, look at tangent vector from previous step, creates normal vector to current position and place vertices along that normal according to trail width.
Next it looks to at previous two vertexes and align them according to average of current and previous tangent, creating trapezoid.
I suggest to leave calculation of fading on GPU (effectively using approach of GPU particles).
If you know velocity vector of object when you are calling update of trail you can use it to optimize that algorithm. Use of dynamic vertex buffer is probably without saying (just use your own vertex format that will include current time at moment when you create those vertices so you can fade it on GPU).
One way could be that you create a list of trails/particles, and you init that on every frame or how much you like. i will try to explain in pseudo code below. i also rotate a bit every trail, and use different size and color of smoke texture, and added a bit of ofsset +- 5 pixels on init.
class Trail
position as vector2d
duration as single
velocity as vector2d
fade as integer = 1
active = true
end class
class Trails
inherits list of Trail
sub Init(position as vector2d, velocity as vector2d)
// add trail to list
end sub
sub Update()
for each trail in me.findAll(function(c) c.active))
position += velocity
fade -= .05 // or some value
end for
me.removeAll(function(c) not(c.active)) // remove from list when unused
end sub
sub Draw()
for each trail in me.findAll(function(c) c.active))
draw(smokeTexture, position, size, rotate, color * trail.fade)
end for
end sub
end class
by this i have achieved this effect, it's barely visible but it gives effect.
If I have two points p1 and p2 where p1 is the pivot point and p2 is the original direction the user was headed and they have a number of possible directions to go p3...pn in random sequence. How do I get the angles between the choices and the segment formed by p1,p2 as clockwise(right hand) positive values between 0 and 360 so that I can sort them from least to greatest?
Also the points p1...pn will be in any quadrant, I can’t assume they will always be in the positive x,y direction. The grid is a standard Cartesian grid not screen coordinates so Y gets smaller as you go down not larger.
So in this example (sorry for the poor drawing but Paint was all I had on my laptop) I need to get the angles:
(p2-p1-p3)
( p2-p1-p4)
( p2-p1-p5)
( p2-p1-p6)
In this order(smallest right hand turn to largest right hand turn):
[( p2-p1-p4), ( p2-p1-p6), ( p2-p1-p5), (p2-p1-p3)]
The points in my case are a class called Vertex:
public class Vertex
{
public double X = 0;
public double Y = 0;
public Vertex() { }
public Vertex(double x, double y)
{
X = x;
Y = y;
}
}
And the code for getting the angles and sorting looks like this right now but has a problem:
private static IEnumerable<Vertex> SortByAngle(Vertex original, Vertex pivot, List<Vertex> choices)
{
choices.Sort((v1, v2) => GetTurnAngle(original, pivot, v1).CompareTo(GetTurnAngle(original, pivot, v2)));
return choices;
}
private static double GetTurnAngle(Vertex original, Vertex pivot, Vertex choice)
{
var a = original.X - pivot.X;
var b = original.Y - pivot.Y;
var c = choice.X - pivot.X;
var d = choice.Y - pivot.Y;
var rads = Math.Acos(((a * c) + (b * d)) / ((Math.Sqrt(a * a + b * b)) * (Math.Sqrt(c * c + d * d))));
return (180 / Math.PI * rads);
}
The problem is the above is if I check it for:
original 66,-66
pivot 280,-191
choice 200,-180
I get an angle of 22.460643124 instead of 337.539356876 which means it went counter-clockwise from the original direction to get that angle. I need it to always go clockwise to get the angle.
What am I doing wrong and how do I fix it?
Update: OK so according to what you guys are saying I can probably use some cross product like math to determine CW vs CCW so the new method would look like this:
private static double GetTurnAngle(Vertex original, Vertex pivot, Vertex choice)
{
var a = original.X - pivot.X;
var b = original.Y - pivot.Y;
var c = choice.X - pivot.X;
var d = choice.Y - pivot.Y;
var angle = Math.Acos(((a * c) + (b * d)) / ((Math.Sqrt(a * a + b * b)) * (Math.Sqrt(c * c + d * d))));
angle = (180 / Math.PI * angle);
var z = (choice.X - pivot.X) * (original.Y - pivot.Y) - (choice.Y - pivot.Y) * (original.X - pivot.X);
if (z < 0)
{
return 360 - angle;
}
return angle;
}
Update 2:
Using the accepted solution it now looks like so:
private static double GetTurnAngle(Vertex original, Vertex pivot, Vertex choice)
{
var angle1 = Math.Atan2(original.Y - pivot.Y, original.X - pivot.X);
var angle2 = Math.Atan2(choice.Y - pivot.Y, choice.X - pivot.X);
var angleDiff = (180 / Math.PI * (angle2 - angle1));
if (angleDiff > 0)//It went CCW so adjust
{
return 360 - angleDiff;
}
return -angleDiff;//I need the results to be always positive so flip sign
}
So far as I can tell that works great so far. Thank you guys for the help!
Take a look at atan2 function. It takes delta y and delta x, so can distinguish all angles.
angle1 = atan2(p1.y-p0.y, p1.x-p0.x);
angle2 = atan2(p2.y-p0.y, p2.x-p0.x);
angle = angle2 - angle1;
If angle is negative, then CW, if positive CCW (or other way around depending on your axis orientation). Note |angle| may be > 180, in which case you may want to do 360-|angle| and reverse the CW CCW conclusion if you're after the shortest route.
You find the Dn=direction from p1 to pn (x=pn.x-p1.x and y=pn.y-p1.y) by the formula:
Dn=f(x,y)=180-90*(1+sign(y))* (1-sign(x^2))-45*(2+sign(y))*sign(x)
-180/pi()*sign(x*y)*atan((abs(y)-abs(x))/(abs(y)+abs(x)))
So the angles are Angle(p2-p1-pn)=Dn-D2.
How can i calculate an arc through 3 points A, B, C in 3d. from A to C passing B (order is taken care of).
Most robot arms have this kind of move command. I need to simulate it and apply different speed dynamics to it and need therefore a parameter 0..1 which moves a position from A to C.
EDIT:
what i have is radius and center of the arc, but how can i parameterize the circle in 3d if i know the start and end angle?
EDIT2:
getting closer. if i have two unit length perpendicular vectors v1 and v2 on the plane in which the circle lies, i can do a parameterization like: x(t) = c + r * cos(t) * v1 + r * sin(t) * v2
so i take v1 = a-c and i only need to find v2 now. any ideas?
Martin Doms recently wrote a blog entry about splines and bezier curves that you might find useful.
Part of his post describes how to get a 2D curve defined by the three control points P0, P1, and P2. The curve is parameterized by a value t that ranges from 0 to 1:
F(t) = (1-t)2 P0 + 2t (1-t) P1 + t2 P2
It seems likely that you could adapt that to 3D with a little thought. (Of course, bezier curves don't necessarily go through the control points. This may not work if that's a deal-breaker for you.)
As an aside, Jason Davies put together a nice little animation of curve interpolation.
Got back to this one and it was quite tricky. The code is as short as possible, but still much more than i thought.
You can create an instance of this class and call the SolveArc method with the 3 positions (in the right order) to set up the class. Then the Arc Method will give you the positions on the circular arc from 0..1 in linear velocity. If you find a shorter solution, please let me know.
class ArcSolver
{
public Vector3D Center { get; private set; }
public double Radius { get; private set; }
public double Angle { get; private set; }
Vector3D FDirP1, FDirP2;
//get arc position at t [0..1]
public Vector3D Arc(double t)
{
var x = t*Angle;
return Center + Radius * Math.Cos(x) * FDirP1 + Radius * Math.Sin(x) * FDirP2;
}
//Set the points, the arc will go from P1 to P3 though P2.
public bool SolveArc(Vector3D P1, Vector3D P2, Vector3D P3)
{
//to read this code you need to know that the Vector3D struct has
//many overloaded operators:
//~ normalize
//| dot product
//& cross product, left handed
//! length
Vector3D O = (P2 + P3) / 2;
Vector3D C = (P1 + P3) / 2;
Vector3D X = (P2 - P1) / -2;
Vector3D N = (P3 - P1).CrossRH(P2 - P1);
Vector3D D = ~N.CrossRH(P2 - O);
Vector3D V = ~(P1 - C);
double check = D|V;
Angle = Math.PI;
var exist = false;
if (check != 0)
{
double t = (X|V) / check;
Center = O + D*t;
Radius = !(Center - P1);
Vector3D V1 = ~(P1 - Center);
//vector from center to P1
FDirP1 = V1;
Vector3D V2 = ~(P3 - Center);
Angle = Math.Acos(V1|V2);
if (Angle != 0)
{
exist = true;
V1 = P2-P1;
V2 = P2-P3;
if ((V1|V2) > 0)
{
Angle = Math.PI * 2 - Angle;
}
}
}
//vector from center to P2
FDirP2 = ~(-N.CrossRH(P1 - Center));
return exist;
}
}
So this answer is part of the story, given that the code is in Mathematica rather than C#, but certainly all of the maths (with perhaps one minor exception) should all be relatively easy to translate to any language.
The basic approach presented is to:
Project the three points (A, B, C) onto the plane that those points lie in. It should have a normal AB x BC. This reduces the problem from three dimensions to two.
Use your favourite technique for finding the center of the circle that passes through the three projected points.
Unproject the center of the circle back into three dimensions.
Use an appropriate spherical interpolation strategy (slerp is used in the sample, but I believe it would have been better to use quaternions).
The one caveat is that you need to work out which direction you are rotating in, I'm sure there are smarter ways, but with only two alternatives, rejection testing is sufficient. I'm using reduce, but you'd probably need to do something slightly different to do this in C#
No guarantees that this is the most numerically stable or robust way to do this, or that there are any corner cases that have been missed.
(* Perpendicular vector in 2 dimensions *)
Perp2d[v_] := {-v[[2]], v[[1]]};
(* Spherical linear interpolation. From wikipedia \
http://en.wikipedia.org/wiki/Slerp *)
slerp[p0_, p1_, t_, rev_] :=
Module[{\[CapitalOmega], v},
\[CapitalOmega] = ArcCos[Dot[p0, p1]];
\[CapitalOmega] =
If[rev == 0, 2 Pi - \[CapitalOmega], \[CapitalOmega]];
v = (Sin[(1 - t) \[CapitalOmega]]/
Sin[\[CapitalOmega]]) p0 + (Sin[t \[CapitalOmega]]/
Sin[\[CapitalOmega]]) p1;
Return[v]
];
(* Based on the expressions from mathworld \
http://mathworld.wolfram.com/Line-LineIntersection.html *)
IntersectionLineLine[{x1_, y1_}, {x2_, y2_}, {x3_, y3_}, {x4_, y4_}] :=
Module[{x, y, A, B, C},
A = Det[{{x1, y1}, {x2, y2}}];
B = Det[{{x3, y3}, {x4, y4}}];
C = Det[{{x1 - x2, y1 - y2}, {x3 - x4, y3 - y4}}];
x = Det[{{A, x1 - x2}, {B, x3 - x4}}]/C;
y = Det[{{A, y1 - y2}, {B, y3 - y4}}]/C;
Return[{x, y}]
]
(* Based on Paul Bourke's Notes \
http://local.wasp.uwa.edu.au/~pbourke/geometry/circlefrom3/ *)
CircleFromThreePoints2D[v1_, v2_, v3_] :=
Module[{v12, v23, mid12, mid23, v12perp, v23perp, c, r},
v12 = v2 - v1;
v23 = v3 - v2;
mid12 = Mean[{v1, v2}];
mid23 = Mean[{v2, v3}];
c = IntersectionLineLine[
mid12, mid12 + Perp2d[v12],
mid23, mid23 + Perp2d[v23]
];
r = Norm[c - v1];
Assert[r == Norm[c - v2]];
Assert[r == Norm[c - v3]];
Return[{c, r}]
]
(* Projection from 3d to 2d *)
CircleFromThreePoints3D[v1_, v2_, v3_] :=
Module[{v12, v23, vnorm, b1, b2, va, vb, vc, xc, xr, yc, yr},
v12 = v2 - v1;
v23 = v3 - v2;
vnorm = Cross[v12, v23];
b1 = Normalize[v12];
b2 = Normalize[Cross[v12, vnorm]];
va = {0, 0};
vb = {Dot[v2, b1], Dot[v2, b2]};
vc = {Dot[v3, b1], Dot[v3, b2]};
{xc, xr} = CircleFromThreePoints2D[va, vb, vc];
yc = xc[[1]] b1 + xc[[2]] b2;
yr = Norm[v1 - yc];
Return[{yc, yr, b1, b2}]
]
v1 = {0, 0, 0};
v2 = {5, 3, 7};
v3 = {6, 4, 2};
(* calculate the center of the circle, radius, and basis vectors b1 \
and b2 *)
{yc, yr, b1, b2} = CircleFromThreePoints3D[v1, v2, v3];
(* calculate the path of motion, given an arbitrary direction *)
path = Function[{t, d},
yc + yr slerp[(v1 - yc)/yr, (v3 - yc)/yr, t, d]];
(* correct the direction of rotation if necessary *)
dirn = If[
TrueQ[Reduce[{path[t, 1] == v2, t >= 0 && t <= 1}, t] == False], 0,
1]
(* Plot Results *)
gr1 = ParametricPlot3D[path[t, dirn], {t, 0.0, 1.0}];
gr2 = ParametricPlot3D[Circle3d[b1, b2, yc, yr][t], {t, 0, 2 Pi}];
Show[
gr1,
Graphics3D[Line[{v1, v1 + b1}]],
Graphics3D[Line[{v1, v1 + b2}]],
Graphics3D[Sphere[v1, 0.1]],
Graphics3D[Sphere[v2, 0.1]],
Graphics3D[{Green, Sphere[v3, 0.1]}],
Graphics3D[Sphere[yc, 0.2]],
PlotRange -> Transpose[{yc - 1.2 yr, yc + 1.2 yr}]
]
Which looks something like this:
I have 3 particles and one of them is the center particle. I want to rotate other two particle ( stored in particles list ) relative to the center particle with the formula q' = Θq + p where q' is the new position of the rotated particle, Θ is the orientation angle and p is the position of center particle. The initial position of other two particles is stored in initialParticlePosition list. THe problem is I think the angle I calculate is wrong because of the range. I thing I should take the range as [-pi, pi) or something like this. In some parts it calculates correct but sometimes it is wrong. Can someone help me with this code or give me another method of rotating.
{
angle = Math.Acos(Vector2.Dot(heading,new Vector2(0,-1) ));
for (int i = 0; i < 2; i++)
{
tempX = (double)initialParticlePositions[i].X * Math.Cos(angle) - (double)initialParticlePositions[i].Y * Math.Sin(angle) + centerParticle.position.x;
tempY = (double)initialParticlePositions[i].X * Math.Sin(angle) + (double)initialParticlePositions[i].Y * Math.Cos(angle) + centerParticle.position.y;
particles[i].position.x = tempX;
particles[i].position.y = tempY;
}
}
Some methods that might help (angles always in degrees, not rad):
public static double GetAngle(Vector v)
{
return Math.Atan2(v.X, -v.Y) * 180.0 / Math.PI;
}
public static Vector SetAngle(Vector v, double angle)
{
var angleInRads = angle * (Math.PI / 180.0);
var distance = v.Length;
v.X = (Math.Sin(angleInRads) * distance);
v.Y = -(Math.Cos(angleInRads) * distance);
return v;
}
static public Point RotatePointAroundCenter(Point point, Point center, double rotationChange)
{
Vector centerToPoint = point - center;
double angle = GetAngle(centerToPoint);
Vector centerToNewPoint = SetAngle(centerToPoint, angle + rotationChange);
return center + centerToNewPoint;
}
(You should start marking answers that help as answer, click the checkmark outline below the votes on the left, e.g. you could accept this answer)
Edit: Optimized the methods a bit.
The particle positions that are orbiting can be set with a single line of code each:
Assume p1, p2, & p3 are Vector2s and p2 & p3 are orbiting p1.
p2 = Vector2.Transform(p2 - p1, Matrix.CreateRotationZ(rotationChangeP2)) + p1;
p3 = Vector2.Transform(p3 - p1, Matrix.CreateRotationZ(rotationChangeP3)) + p1;
The Matrix.Create...() method will call the two trig functions for you.
edit. the Matrix & Vector2 structures & methods are XNA specific but included here because that's what the OP tagged his Q with.
angle = Math.Acos(Vector2.Dot(heading,new Vector2(0,-1)));
As you suspect, your combination of dot product and Acos will only give you angles in a 180
degree range.
Instead, use Atan2 on your unit vector to get a full range of angles from -pi to pi.
angle = (float)Math.Atan2((double)heading.Y, (double)heading.X);
You may need to negate the Y term if your Y axis is positive in the down direction.