find center of circle when three points are given - algorithm

I studied this link and coded accordingly but getting Wrong Answer for the example explained in the link,
During solving the equation, I subtracted equation 2 from equation 1 and equation 3 from equation 2 and then proceed further. Please check link for clarification.
My code is:
include<stdio.h>
int is_formCircle(float a1,float b1,float a2,float b2,float a3,float b3) {
float p1=a2-a1;
float p2=a3-a2;
float p3=b2-b1;
float p4=b3-b2;
float alpha=(a1+a2)*(a1-a2) + (b1+b2)*(b1-b2);
float beta =(a2+a3)*(a2-a3) + (b2+b3)*(b2-b3);
float y1=p1*beta - p2*alpha;
float y2=p2*p3 - p1*p4;
if(y2==0 || y1==0) return 1;
float y=y1/y2;
float x1 = 2*p4*y + beta;
float x2 = 2*p2;
float x = x1/x2;
printf("x=%f y=%f\n",x,y);
return 0;
}
int main() {
float a1,a2,a3,a4,b1,b2,b3,b4;
a1=4.0;
b1=1.0;
a2=-3.0;
b2=7.0;
a3=5.0;
b3=-2.0;
is_formCircle(a1,b1,a2,b2,a3,b3);
return 0;
}
MY another Code:
#include<stdio.h>
int is_formCircle(float a1,float b1,float a2,float b2,float a3,float b3) {
float mid1,mid2,mid3,mid4,m1,m2,D,Dx,Dy,x,y;
mid1 = a1+(a2-a1)/2;
mid2 = b1+(b2-b1)/2;
mid3 = a2+(a3-a2)/2;
mid4 = b2+(b3-b2)/2;
m1=(b2-b1)/(a2-a1);
m2=(b3-b2)/(a3-a2);
m1=-1*m1;
m2=-1*m2;
D=m2-m1;
Dx=mid2-(m1*mid1) + (mid3*m2) - mid4;
Dy=(m1*(mid3*m2-mid4))-(m2*(mid1*m1-mid2));
x=Dx/D;
y=Dy/D;
printf("%f %f",x,y);
return 0;
}
int main() {
float a1,a2,a3,b1,b2,b3;
a1=4.0;
b1=1.0;
a2=-3.0;
b2=7.0;
a3=5.0;
b3=-2.0;
is_formCircle(a1,b1,a2,b2,a3,b3);
return 0;
}
Why my code giving Wrong Answer?

I have to say, if you're following the link you listed, it would've helped to keep the variable names the same. We could understand the algorithm much better seeing x1, y1, x2, y2, x3, y3 instead of p1, p2, p3, p4, alpha and beta. In fact, I don't see much in your algorithm that matches the link. I'm not trying to be as harsh as the comments were (and if you're worried about switching float to double, that was a perfectly good case for a typedef), but debugging algorithms is easiest when you don't have to convert variable names.
I would recommend simply using what they give you for h and k in the link, which is namely done by calculating determinants of 3x3 matrices. You can find lots of references for that.
I'd make two functions, as follows:
float calculateH(float x1, float y1, float x2, float y2, float x3, float y3) {
float numerator = (x2*x2+y2*y2)*y3 - (x3*x3+y3*y3)*y2 -
((x1*x1+y1*y1)*y3 - (x3*x3+y3*y3)*y1) +
(x1*x1+y1*y1)*y2 - (x2*x2+y2*y2)*y1;
float denominator = (x2*y3-x3*y2) -
(x1*y3-x3*y1) +
(x1*y2-x2*y1);
denominator *= 2;
return numerator / denominator;
}
float calculateK(float x1, float y1, float x2, float y2, float x3, float y3) {
float numerator = x2*(x3*x3+y3*y3) - x3*(x2*x2+y2*y2) -
(x1*(x3*x3+y3*y3) - x3*(x1*x1+y1*y1)) +
x1*(x2*x2+y2*y2) - x2*(x1*x1+y1*y1);
float denominator = (x2*y3-x3*y2) -
(x1*y3-x3*y1) +
(x1*y2-x2*y1);
denominator *= 2;
return numerator / denominator;
}
Then your is_formCircle would simply be:
float is_formCircle(float x1, float y1, float x2, float y2, float x3, float y3) {
float h = calculateH(x1, y1, x2, y2, x3, y3);
float k = calculateK(x1, y1, x2, y2, x3, y3);
printf("x=%f y=%f\n",h,k);
}
There are tons of ways to optimize this, and there's a chance I typoed any of the determinant calculations, but it should get you going.

The solution that was given in the link is a "blind" solution, i.e., you know the equation, boom solve it.
However, if you understand more deeply what is behind the scene, you will be able to:
Write a more readable, reliable, flexible code.
Debug easily.
What happen when you substract equation 1 from equation 2? You actually try to find the equation of the straight line describing those points which are equidistant from the point 1 and the point 2. Then, you do the same with point 2 and 3. Finally, you find the intersection between these to lines, which gives you the center of the circle.
How do you describe the straight line of the points equidistant to the point 1 and 2? You take the point that is in the middle of the two, and go in the direction perpendicular to the direction between point 1 and 2.
If this is not absolutly clear, take a paper and draw an example: put points 1,2 and 3, find the two lines and find the intersection.
Now that you understood everything, reshape your code with two functions, on that find the line equidistant between two points, another one which compute the intersection between two lines...
After your edit, the code looked better, although it was not simple to understand. I think that the mistake is when you solve for the intersection of the two lines, do not forget that you are under parametric form:
Dx = (mid4-mid2) - m2*(mid3-mid1);
lambda=Dx/D;
x = mid1 + lambda*m1;
y = mid2 + lambda*1.0;
Checked graphically using Matlab.

Related

Sorting an array of objects based on one attribute only in Processing

I have a series of randomly plotted lines from a class called Line.
I have put all the objects into an array. I would like to connect any lines that are near each other with a dotted line. The simplest way I can think of doing this is to say if the x1 co-ordinate is <5 pixels from the x1 of another line, then draw a dotted line connecting the two x1 co-ordinates.
The problem I have is how to compare all the x1 co-ordinates with all the other x1 co-ordinates. I think this should involve 1. Sorting the array and then 2. Comparing consecutive array elements. However I want to sort only on x1 and I dont know how to do this.
Here is my code so far:
class Line{
public float x1;
public float y1;
public float x2;
public float y2;
public color cB;
public float rot;
public float fat;
public Line(float x1, float y1, float x2, float y2, color tempcB, float rot, float fat){
this.x1 = x1;
this.y1 = y1;
this.x2 = x2;
this.y2 = y2;
this.cB = tempcB;
this.rot = rot;
this.fat = fat;
};void draw(){
line(x1, y1, x2, y2);
//float rot = random(360);
float fat = random(5);
strokeWeight(fat);
////stroke (red,green,blue,opacity)
stroke(fat*100, 0, 0);
rotate(rot);
}
}
//Create array of objects
ArrayList<Line> lines = new ArrayList<Line>();
void setup(){
background(204);
size(600, 600);
for(int i = 0; i < 200; i++){
float r = random(500);
float s = random(500);
lines.add(new Line(r,s,r+10,s+10,color(255,255,255),random(360),random(5)));
}
//Draw out all the lines from the array
for(Line line : lines){
line.draw();
//Print them all out
println(line.x1,line.y1,line.x2,line.y2,line.cB,line.rot,line.fat);
}
}
//Now create connections between the elements
//If the x1 of the line is <5 pixels from another line then create a dotted line between the x1 points.
Like the other answer said, you need to compare both end points for this to make any sense. You also don't have to sort anything.
You should be using the dist() function instead of trying to compare only the x coordinate. The dist() function takes 2 points and gives you their distance. You can use this to check whether two points are close to each other or not:
float x1 = 75;
float y1 = 100;
float x2 = 25;
float y2 = 125;
float distance = dist(x1, y1, x2, y2);
if(distance < 100){
println("close");
}
You can use this function in your Line class to loop through other Lines and check for close points, or find the closest points, whatever you want.
As always, I recommend you try something out and ask another question if you get stuck.
The problem lies in the fact that a Line is composed of two points, and despite being tied together (pun intended), you need to check the points of each Line independently. The only point you really don't need to check is other point in the same Line instance.
In this case, it might be in your best interest to have a Point class. Line would then use Point instances to define both ends rather than the raw float coordinates. In this way, you can have both a list of Lines as well as a list of Points.
In this way you can sort Points by x coordinate or y coordinate and grab all points within 5 pixels of your point (and that isn't the same instance or other point in Line instance of course).
Being able to split handling into Points and Lines is important in that you're using multiple views to handle the same data. As a general rule, you should rearrange said data whenever it becomes cumbersome to deal with in its current form. However if I may make a recommendation, the sorting is not strictly necessary. If you're checking a single point with all other points, you'd have to sort repeatedly according to the current point which is more work than simply making a pass in a list to deal with all other points that are close enough.

Moving along Bezier Curve in processing

My code for the ball moving in a Bezier Curve from start to the middle of the curve is:
void ballMove()
{
if(y[0]==height*1/10)
{
bezier (x[0], y[0],x[1], y[1], x[2], y[2], x[3], y[3]);
float x0; float x1; float x2; float x3;
float y0; float y1; float y2; float y3;
x0 = x[0]; x1 = x[1]; x2 = x[2]; x3 = x[3];
y0 = y[0]; y1 = y[1]; y2 = y[2]; y3 = y[3];
float t = (frameCount/100.0)%1;
float x = bezierPoint(x0, x1, x2, x3, t);
float y = bezierPoint( y0, y1, y2, y3, t);
if(t>=0.5)
{
t=0;
}
while(t==0.5)
{
a=x;
b=y;
}
while(t>0.5)
{
ellipse(a,b,30,30);
}
fill(255,0,0);
if(t!=0)
{
ellipse(x, y, 15, 15);
}
}
}
I have defined everything in setup, draw etc, but i want to launch the ball from the start to the middle of the Bezier Curve only one time whenever space is pressed.
The current version shows me the loop. How can i do that?
Tried Everything like return, break, changing the t parameter etc, but the code doesn't work. I'm new in processing.
Do you have any suggestions?
Biggest mistake that you make is altering value of t after you calculated x and y positions of red ball. To avoid this you need first calculate t between [0, 1] in you case [0, 0.5] and then alter this value according to state of your program.
Second mistake you made while calculating t from frameCount. First you use modulo to extract numbers [0, 50] and then map it in range [0, 0.5] like this
float t = (frameCount % 50) * 0.01;
You also mentioned that you want to repeat this animation after pressing some key. For this you will need keyPressed method and some global variables to represent state of program and store starting frame of animation (because frameCount should be read only). So basic functionality can be achieved like this:
boolean run = false;
float f_start = 0;
void ballMove() {
noFill();
bezier (x0, y0, x1, y1, x2, y2, x3, y3);
float t = ((frameCount - f_start) % 50) * 0.01;
if (run == false) {
t = 0;
}
float x = bezierPoint(x0, x1, x2, x3, t);
float y = bezierPoint( y0, y1, y2, y3, t);
fill(255, 0, 0);
ellipse(x, y, 5, 5);
}
void keyPressed() {
run = !run;
f_start = frameCount;
}
Hope this will help you. Next time pls post an MCVE so we do not need to fight with your code.

Linear interpolation along a "Bresenham line"

I'm using linear interpolation for animating an object between two 2d coordinates on the screen. This is pretty close to what I want, but because of rounding, I get a jagged motion. In ASCII art:
ooo
ooo
ooo
oo
Notice how it walks in a Manhattan grid, instead of taking 45 degree turns. What I'd like is linear interpolation along the line which Bresenham's algorithm would have created:
oo
oo
oo
oo
For each x there is only one corresponding y. (And swap x/y for a line that is steep)
So why don't I just use Bresenham's algorithm? I certainly could, but that algorithm is iterative, and I'd like to know just one coordinate along the line.
I am going to try solving this by linearly interpolating the x coordinate, round it to the pixel grid, and then finding the corresponding y. (Again, swap x/y for steep lines). No matter how that solution pans out, though, I'd be interested in other suggestion and maybe previous experience.
Bresenham's algorithm for lines was introduced to draw a complete line a bit faster than usual approaches. It has two major advantages:
It works on integer variables
It works iteratively, which is fast, when drawing the complete line
The first advantage is not a great deal, if you calculate only some coordinates. The second advantage turns out as a disadvantage when calculating only some coordinates. So after all, there is no need to use Bresenham's algorithm.
Instead, you can use a different algorithm, which results in the same line. For example the DDA (digital differential analyzer). This is basically, the same approach you mentioned.
First step: Calculate the slope.
m = (y_end - y_start) / (x_end - x_start)
Second step: Calculate the iteration step, which is simply:
i = x - x_start
Third step: Calculate the coresponding y-value:
y = y_start + i * m
= y_start + (x - x_start) * (y_end - y_start) / (x_end - x_start)
Here's the solution I ended up with:
public static Vector2 BresenhamLerp(Vector2 a, Vector2 b, float percent)
{
if (a.x == b.x || Math.Abs(a.x - b.x) < Math.Abs(a.y - b.y))
{
// Didn't do this part yet. Basically, we just need to recurse
// with x/y swapped and swap result on return
}
Vector2 result;
result.x = Math.Round((1-percent) * a.x + percent * b.x);
float adjustedPercent = (result.x - a.x + 0.5f) / (b.x - a.x);
result.y = Math.Round((1-adjustedPercent) * a.y + adjustedPercent * b.y);
return result;
}
This is what I just figured out would work. Probably not the most beautiful interpolations, but it is just a 1-2 float additions per iteration on the line with a one-time precalculation. Works by calculating the number of steps on a manhattan matrix.
Ah, and it does not yet catch the case when the line is vertical (dx = 0)
This is the naive bresenham, but the iterations could in theory only use integers as well. If you want to get rid of the float color value, things are going to get harder because the line might be longer than the color difference, so delta-color < 1.
void Brepolate( uint8_t* pColorBuffer, uint8_t cs, float xs, float ys, float zs, uint8_t ce, float xe, float ye, float ze )
{
float nColSteps = (xe - xs) + (ye - ys);
float fColInc = ((float)cs - (float)ce) / nColSteps;
float fCol = cs;
float dx = xe - xs;
float dy = ye - ys;
float fCol = cs;
if (dx > 0.5)
{
float de = fabs( dy / dx );
float re = de - 0.5f;
uint32_t iY = ys;
uint32_t iX;
for ( uint32_t iX = xs;
iX <= xe;
iX++ )
{
uint32_t off = surf.Offset( iX, iY );
pColorBuffer[off] = fCol;
re += de;
if (re >= 0.5f)
{
iY++;
re -= 1.0f;
fCol += fColInc;
}
fCol += fColInc;
}
}
}

Draw a sphere using 3D pixels (voxels)

Can you suggest an algorithm that can draw a sphere in 3D space using only the basic plot(x,y,z) primitive (which would draw a single voxel)?
I was hoping for something similar to Bresenham's circle algorithm, but for 3D instead of 2D.
FYI, I'm working on a hardware project that is a low-res 3D display using a 3-dimensional matrix of LEDs, so I need to actually draw a sphere, not just a 2D projection (i.e. circle).
The project is very similar to this:
... or see it in action here.
One possibility I have in mind is this:
calculate the Y coordinates of the poles (given the radius) (for a sphere centered in the origin, these would be -r and +r)
slice the sphere: for each horizontal plane pi between these coordinates, calculate the radius of the circle obtained by intersecting said plane with the sphere => ri.
draw the actual circle of radius ri on plane pi using Bresenham's algorithm.
FWIW, I'm using a .NET micro-framework microprocessor, so programming is C#, but I don't need answers to be in C#.
The simple, brute force method is to loop over every voxel in the grid and calculate its distance from the sphere center. Then color the voxel if its distance is less than the sphere radius. You can save a lot of instructions by eliminating the square root and comparing the dot product to the radius squared.
Pretty far from optimal, sure. But on an 8x8x8 grid as shown, you'll need to do this operation 512 times per sphere. If the sphere center is on the grid, and its radius is an integer, you only need integer math. The dot product is 3 multiplies and 2 adds. Multiplies are slow; let's say they take 4 instructions each. Plus you need a comparison. Add in the loads and stores, let's say it costs 20 instructions per voxel. That's 10240 instructions per sphere.
An Arduino running at 16 MHz could push 1562 spheres per second. Unless you're doing tons of other math and I/O, this algorithm should be good enough.
I don't believe running the midpoint circle algorithm on each layer will give the desired results once you reach the poles, as you will have gaps in the surface where LEDs are not lit. This may give the result you want, however, so that would be up to aesthetics. This post is based on using the midpoint circle algorithm to determine the radius of the layers through the middle two vertical octants, and then when drawing each of those circles also setting the points for the polar octants.
I think based on #Nick Udall's comment and answer here using the circle algorithm to determine radius of your horizontal slice will work with a modification I proposed in a comment on his answer. The circle algorithm should be modified to take as an input an initial error, and also draw the additional points for the polar octants.
Draw the standard circle algorithm points at y0 + y1 and y0 - y1: x0 +/- x, z0 +/- z, y0 +/- y1, x0 +/- z, z0 +/- x, y0 +/- y1, total 16 points. This forms the bulk of the vertical of the sphere.
Additionally draw the points x0 +/- y1, z0 +/- x, y0 +/- z and x0 +/- x, z0 +/- y1, y0 +/- z, total 16 points, which will form the polar caps for the sphere.
By passing the outer algorithm's error into the circle algorithm, it will allow for sub-voxel adjustment of each layer's circle. Without passing the error into the inner algorithm, the equator of the circle will be approximated to a cylinder, and each approximated sphere face on the x, y, and z axes will form a square. With the error included, each face given a large enough radius will be approximated as a filled circle.
The following code is modified from Wikipedia's Midpoint circle algorithm. The DrawCircle algorithm has the nomenclature changed to operate in the xz-plane, addition of the third initial point y0, the y offset y1, and initial error error0. DrawSphere was modified from the same function to take the third initial point y0 and calls DrawCircle rather than DrawPixel
public static void DrawCircle(int x0, int y0, int z0, int y1, int radius, int error0)
{
int x = radius, z = 0;
int radiusError = error0; // Initial error state passed in, NOT 1-x
while(x >= z)
{
// draw the 32 points here.
z++;
if(radiusError<0)
{
radiusError+=2*z+1;
}
else
{
x--;
radiusError+=2*(z-x+1);
}
}
}
public static void DrawSphere(int x0, int y0, int z0, int radius)
{
int x = radius, y = 0;
int radiusError = 1-x;
while(x >= y)
{
// pass in base point (x0,y0,z0), this algorithm's y as y1,
// this algorithm's x as the radius, and pass along radius error.
DrawCircle(x0, y0, z0, y, x, radiusError);
y++;
if(radiusError<0)
{
radiusError+=2*y+1;
}
else
{
x--;
radiusError+=2*(y-x+1);
}
}
}
For a sphere of radius 4 (which actually requires 9x9x9), this would run three iterations of the DrawCircle routine, with the first drawing a typical radius 4 circle (three steps), the second drawing a radius 4 circle with initial error of 0 (also three steps), and then the third drawing a radius 3 circle with initial error 0 (also three steps). That ends up being nine calculated points, drawing 32 pixels each.
That makes 32 (points per circle) x 3 (add or subtract operations per point) + 6 (add, subtract, shift operations per iteration) = 102 add, subtract, or shift operations per calculated point. In this example, that's 3 points for each circle = 306 operations per layer. The radius algorithm also adds 6 operations per layer and iterates 3 times, so 306 + 6 * 3 = 936 basic arithmetic operations for the example radius of 4.
The cost here is that you will repeatedly set some pixels without additional condition checks (i.e. x = 0, y = 0, or z = 0), so if your I/O is slow you may be better off adding the condition checks. Assuming all LEDs were cleared at the start, the example circle would set 288 LEDs, while there are many fewer LEDs that would actually be lit due to repeat sets.
It looks like this would perform better than the bruteforce method for all spheres that would fit in the 8x8x8 grid, but the bruteforce method would have consistent timing regardless of radius, while this method will slow down when drawing large radius spheres where only part will be displayed. As the display cube increases in resolution, however, this algorithm timing will stay consistent while bruteforce will increase.
Assuming that you already have a plot function like you said:
public static void DrawSphere(double r, int lats, int longs)
{
int i, j;
for (i = 0; i <= lats; i++)
{
double lat0 = Math.PI * (-0.5 + (double)(i - 1) / lats);
double z0 = Math.Sin(lat0) * r;
double zr0 = Math.Cos(lat0) * r;
double lat1 = Math.PI * (-0.5 + (double)i / lats);
double z1 = Math.Sin(lat1) * r;
double zr1 = Math.Cos(lat1) * r;
for (j = 0; j <= longs; j++)
{
double lng = 2 * Math.PI * (double)(j - 1) / longs;
double x = Math.Cos(lng);
double y = Math.Sin(lng);
plot(x * zr0, y * zr0, z0);
plot(x * zr1, y * zr1, z1);
}
}
}
That function should plot a sphere at the origin with specified latitude and longitude resolution (judging by your cube you probably want something around 40 or 50 as a rough guess). This algorithm doesn't "fill" the sphere though, so it will only provide an outline, but playing with the radius should let you fill the interior, probably with decreasing resolution of the lats and longs along the way.
Just found an old q&a about generating a Sphere Mesh, but the top answer actually gives you a short piece of pseudo-code to generate your X, Y and Z :
(x, y, z) = (sin(Pi * m/M) cos(2Pi * n/N), sin(Pi * m/M) sin(2Pi * n/N), cos(Pi * m/M))
Check this Q&A for details :)
procedurally generate a sphere mesh
My solution uses floating point math instead of integer math not ideal but it works.
private static void DrawSphere(float radius, int posX, int poxY, int posZ)
{
// determines how far apart the pixels are
float density = 1;
for (float i = 0; i < 90; i += density)
{
float x1 = radius * Math.Cos(i * Math.PI / 180);
float y1 = radius * Math.Sin(i * Math.PI / 180);
for (float j = 0; j < 45; j += density)
{
float x2 = x1 * Math.Cos(j * Math.PI / 180);
float y2 = x1 * Math.Sin(j * Math.PI / 180);
int x = (int)Math.Round(x2) + posX;
int y = (int)Math.Round(y1) + posY;
int z = (int)Math.Round(y2) + posZ;
DrawPixel(x, y, z);
DrawPixel(x, y, -z);
DrawPixel(-x, y, z);
DrawPixel(-x, y, -z);
DrawPixel(z, y, x);
DrawPixel(z, y, -x);
DrawPixel(-z, y, x);
DrawPixel(-z, y, -x);
DrawPixel(x, -y, z);
DrawPixel(x, -y, -z);
DrawPixel(-x, -y, z);
DrawPixel(-x, -y, -z);
DrawPixel(z, -y, x);
DrawPixel(z, -y, -x);
DrawPixel(-z, -y, x);
DrawPixel(-z, -y, -x);
}
}
}

Calculate direction angle from two vectors?

Say I have two 2D vectors, one for an objects current position and one for that objects previous position. How can I work out the angular direction of travel?
This image might help understand what I'm after:
(image) http://files.me.com/james.ingham/crcvmy
The direction vector of travel will be the difference of the two position vectors,
d = (x1, y1) - (x, y) = (x1 - x, y1 - y)
Now when you ask for the direction angle, that depends what direction you want to measure the angle against. Is it against the x axis? Go with Radu's answer. Against an arbitrary vector? See justjeff's answer.
Edit: To get the angle against the y-axis:
tan (theta) = (x1 -x)/(y1 - y)
the tangent of the angle is the ratio of the x-coordinate of the difference vector to the y-coordinate of the difference vector.
So
theta = arctan[(x1 - x)/(y1 - y)]
Where arctan means inverse tangent. Not to be confused with the reciprocal of the tangent, which many people do, since they're both frequently denoted tan^-1. And make sure you know whether you're working in degrees or radians.
If you're in C (or other language that uses the same function set) then you're probably looking for the atan2() function. From your diagram:
double theta = atan2(x1-x, y1-y);
That angle will be from the vertical axis, as you marked, and will be measured in radians (God's own angle unit).
Be careful to use atan2 to avoid quadrant issues and division by zero. That's what it's there for.
float getAngle(CGPoint ptA, CGPoint ptOrigin, CGPoint ptB)
{
CGPoint A = makeVec(ptOrigin, ptA);
CGPoint B = makeVec(ptOrigin, ptB);
// angle with +ve x-axis, in the range (−π, π]
float thetaA = atan2(A.x, A.y);
float thetaB = atan2(B.x, B.y);
float thetaAB = thetaB - thetaA;
// get in range (−π, π]
while (thetaAB <= - M_PI)
thetaAB += 2 * M_PI;
while (thetaAB > M_PI)
thetaAB -= 2 * M_PI;
return thetaAB;
}
However, if you don't care about whether it's a +ve or -ve angle, just use the dot product rule (less CPU load):
float dotProduct(CGPoint p1, CGPoint p2) { return p1.x * p2.x + p1.y * p2.y; }
float getAngle(CGPoint A, CGPoint O, CGPoint B)
{
CGPoint U = makeVec(O, A);
CGPoint V = makeVec(O, B);
float magU = vecGetMag(U);
float magV = vecGetMag(V);
float magUmagV = magU * magV; assert (ABS(magUmagV) > 0.00001);
// U.V = |U| |V| cos t
float cosT = dotProduct(U, V) / magUmagV;
float theta = acos(cosT);
return theta;
}
Note that in either code section above, if one ( or both ) vectors are close to 0 length this is going to fail. So you might want to trap that somehow.
Still not sure what you mean by rotation matrices, but this is a simple case of getting an azimuth from a direction vector.
The complicated answer:
Normally you should pack a few conversion/utility functions with your 2D vectors: one to convert from X,Y (carthesian) to Theta,R (polar coordinates). You should also support basic vector operations like addition, substraction and dot product.
Your answer in this case would be:
double azimuth = (P2 - P1).ToPolarCoordinate().Azimuth;
Where ToPolarCoordinate() and ToCarhtesianCoordinate() are two reciprocal functions switching from one type of vector to another.
The simple one:
double azimuth = acos ((x2-x1)/sqrt((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1));
//then do a quadrant resolution based on the +/- sign of (y2-y1) and (x2-x1)
if (x2-x1)>0 {
if (y2-y1)<0 { azimuth = Pi-azimuth; } //quadrant 2
} else
{ if (y2-y1)> 0 { azimuth = 2*Pi-azimuth;} //quadrant 4
else { azimuth = Pi + azimuth;} //quadrant 3
}

Resources