I'm making a python script right now that is trying to find the length of an arc, where it given this information:
center of arc: x1, y1
start point of arc: x2, y2
end point of arc: x3, y3
direction, cw, ccw
so far I have been able to successfully calculate the radius, and I tried calculating the angle using the equation:
But for any arcs that have an angle greater than 1*pi or 180 degrees, it returns the incorrect (but correct) inside angle.
What is the correct equation knowing the radius and these three points that I can use to find the value of the angle of the arc from 0 rad/degrees to 360 degrees/2pi radians, going in either the clockwise or counterclockwise direction (it can be either or and I need to be able to calculate for both scenarios)
Code:
# code to find theta
aVector = np.array([x1 - x2, y1 - y2])
bVector = np.array([x1 - x3, y1 - y3])
aMag = np.linalg.norm(aVector)
bMag = np.linalg.norm(aVector)
theta = np.arcos(np.dot(aVector, bVector) / (aMag * bMag))
as you can see here, I'm using arccos which to my dismay only outputs 0-180 degrees
Solution/Working code:
# equation for angle using atan2
start = math.atan2(y2 - y1, x2 - x1)
end = math.atan2(y3 - y1, x3 - x1)
if gcodeAnalysis[tempLineNum][4] == "G3": # going CW
start, end = end, start
tau = 2.0 * math.pi
theta = math.fmod(math.fmod(end - start, tau) + tau, tau)
Working Values:
X1 = 0.00048399999999998444
Y1 = 0.0002720000000007161
X2 = 0.378484
Y2 = -14.694728
X3 = 3.376
Y3 = -14.307
Proper result/value
Theta = 6.077209477545957
Assume this arc was done CCW
As you noticed, the range of math.acos is [0, pi], making it rather useless for telling you the relative directions of the vectors. To get full circular information about a pair of angles, you can use math.atan2. While regular math.atan has a range of [-pi/2, pi/2], atan2 splits the inputs into two parts and returns an angle in the range (-pi, pi]. You can compute the angles relative to any reference, not necessarily relative to each other:
start = math.atan2(y2 - y1, x2 - x1)
end = math.atan2(y3 - y1, x3 - x1)
Now you can use some common formulae to find the difference between the angles in whatever direction you want. I've implemented some of these in a small utility library I made called haggis. The specific function you want is haggis.math.ang_diff_pos.
First, the "manual" computation:
if direction == 'cw':
start, end = end, start
tau = 2.0 * math.pi
angle = math.fmod(math.fmod(end - start, tau) + tau, tau)
If you want to use my function, you can do
if direction == 'cw':
start, end = end, start
angle = ang_diff_pos(start, end)
All of these operations can be easily vectorized using numpy if you find yourself dealing with many points all at once.
You can use the cross product of the two vector to determine if the two vector need to rotate clock or counter-clock wise.
See code below:
import numpy as np
from numpy import linalg as LA
x1 = 0
y1 = 0
x2 = 2
y2 = 0
x3 = 2
y3 = -2
dir = 'ccw' # or ccw
v1 = np.array([x2-x1,y2-y1])
v2 = np.array( [x3-x1,y3-y1])
# if the cross product is positive, then the two vector need to rotate counter clockwise
rot = np.cross(v1,v2)
vdir = 'ccw' if rot >0 else 'cw'
r = (v1[0]*v2[0]+v1[1]*v2[1])/(LA.norm(v1)*LA.norm(v2))
deg = np.arccos(r)/np.pi*180
if vdir != dir:
deg = 360 -deg
print(deg)
I have a course project where I need to write a python algorithm to do the following steps:
1- Take the camera angle, flyings height and sped for the user.
2- Take x and y (represents longitude and altitude) from the user.
3- Calculate the area of the rectangle the drone needs to scan.
4- Divide the area into columns based on the camera angle.
5- Make the drone start from the middle of the first column from top then goes to bottom, then moves half of the next column's width and goes from bottom to top. the repeat till the whole area covered.
6- calculate distance then flying time.
The first 4 steps are straight forward and i added them to clarify the task details.
The algorithm required in the 5th step and then how to calculate the flying time is what I need help at.
I'm new to algorithm writing and coding in python in general, can anyone help on how I can write the algorithm required in step 5
I added the code I wrote to getting the total distance. I keep getting the distance = 0. plus I don't think the distance of the path is correctly calculated (please see the attached screenshot to shows the path)
def distance(height, angle, speed, x1, y1, x2, y2):
xc = (x1 + x2)/2
yc = (y1 + y2)/2
xd = (x1 - x2)/2
yd = (y1 - y2)/2
#Third corner
x3 = xc - yd
y3 = yc + xd
#Fourth corner
x4 = xc + yd
y4 = yc - xd
#width and heigth of the rectangle
width = x2 - x1
length= y2 - y1
#calculating a single column width
column_w = (math.sin(angle/2) * height) * 2
total_columns = int(width / column_w)
total_distance= 0
for i in range(total_columns):
total_distance = total_distance + length
if(i % 2 == 0):
print("drone movement: down, right, up")
else:
print("drone movement: up, right, down")
print(total_distance)
print(legnth)
#To get the inputs from the user
height = int(input("Enter height:"))
angle = int(input("Enter angle:"))
speed = int(input("Enter speed:"))
x1 = int(input("Enter x1:"))
y1 = int(input("Enter y1:"))
x2 = int(input("Enter x2:"))
y2 = int(input("Enter y2:"))
distance(height, angle, speed, x1, y1, x2, y2)
I think this is more to do as a formula rather than an "algorithm".
Your resulting flight path is a series of "S"s starting top left, down, U-turn, back up, U-turn, down and so on and so forth.
From what you've written in step #5, when the drone does a U-turn it goes sideways half the width of a column.
The total "hight" is given by the y input, so h1 is similar to that (minus some for camera angle etc).
The total width of the field is given by the x input (I think).
Assuming we have n columns in it with width w1, we get the number of U-turns:
Each one moves w1/2 sideways and so coverage is (for m sideways moves):
1 + m * (w1/2) = n * w1.
This results in m = 2n - 1 (after rounding up) U-turns.
Works out you'll need 2n * h1 (up/down flights - maybe 2n-1 for odd/even cases) + (2n-1) * w1 sideway fligths.
For a complete path:
direction = 'down'
nextUTurn = 'left'
columnsNeeded = n
for (columnIndex of columnsNeeded) {
print(direction)
print('U-Turn ' + nextUTurn)
direction = getOther(direction)
nextUTurn = getOther(nextUTurn)
}
I would like to make some kind of solar system in pygame. I've managed to do a fixed one but I thought it would be more interesting to do one with planets moving around the sun and moons around planets etc. Is there a way I could do that (using pygame if possible)?
What I would like is :
Sun = pygame.draw.circle(...)
planet1 = pygame.draw.circle(...)
etc.
a = [planet1, planet2, ...]
for p in a:
move p[2] to pos(x, y)
That is what I think would work but I'm not sure how to do it. Also, I've thought about deleting the ancient planet and drawing a new one right next to it, but problem is I'm using random features (like colours, distance to the sun, number of planets in the system etc.) and it would have to keep these same features. Any ideas?
Thanks in advance!
You can implement gravity with Newton's Law of Universal Gravitation and Newton's Second Law to get the accelerations of the planets. Give each planet an initial position, velocity and mass. Acceleration is change in velocity a = v * dt, velocity is change in position v = r * dt, so we can integrate to find velocity and position.
Universal gravitation: F = G * m1 * m2 / r ** 2 where F is the magnitude of the force on the object, G is the gravitational constant, m1 and m2 are the masses of the objects and r is the distance between the two objects.
Newton's Second Law: F = m1 * a where a is the acceleration.
dt = 0.01 # size of time step
G = 100 # gravitational constant
def calcGravity(sun, planet):
'Returns acceleration of planet with respect to the sun'
diff_x = sun.x - planet.x
diff_y = sun.y - planet.y
acceleration = G * sun.mass / (diff_x ** 2 + diff_y ** 2)
accel_x = acceleration * diff_x / (diff_x ** 2 + diff_y ** 2)
accel_y = acceleration * diff_y / (diff_x ** 2 + diff_y ** 2)
return accel_x, accel_y
while True:
# update position based on velocity
planet.x += planet.vel_x * dt
planet.y += planet.vel_y * dt
# update velocity based on acceleration
accel_x, accel_y = calcGravity(sun, planet)
planet.vel_x += accel_x * dt
planet.vel_y += accel_y * dt
This can produce circular and elliptical orbits. Creating an orbiting moon requires a very small timestep (dt) for the numeric integration.
Note: this approach is subtly inaccurate due to the limits of numeric integration.
Sample implementation in pygame here, including three planets revolving around a sun, a moon, and a basic orbital transfer.
https://github.com/c2huc2hu/orbital_mechanics
Coordinates of a planet rotated about the Sun through some angle with respect to the X-axis are , where r is the distance to the Sun, theta is that angle, and (a, b) are the coordinates of the sun. Draw your circle centered at (x, y).
EDIT:
General elliptical orbit:
Where
r0 is the radius of a circular orbit with the same angular momentum, and e is the "eccentricity" of the ellipse
I'm programming a function in Python in Autodesk Maya (using PyMel for Maya)
I have three 3D points; p0, p1, p2.
Then they make a rigid transformation, so after the transformation (an affine transformation) I have their new positions; q0, q1, q2.
I also have a fourth point before the transformation; p3. I want to calculate its position after the same transformation; q4.
So I need to calculate the transformation matrix, and then apply it to p4. I don't know how to do either. List = an array of objects
import pymel.core as pm
import pymel.core.datatypes as dt
p0 = dt.Vector(pm.getAttr(list[0]+".tx"), pm.getAttr(list[0]+".ty"), pm.getAttr(list[0]+".tz"))
p1 = dt.Vector(pm.getAttr(list[1]+".tx"), pm.getAttr(list[1]+".ty"), pm.getAttr(list[1]+".tz"))
p2 = dt.Vector(pm.getAttr(list[2]+".tx"), pm.getAttr(list[2]+".ty"), pm.getAttr(list[2]+".tz")
p3 = dt.Vector(pm.getAttr(list[3]+".tx"), pm.getAttr(list[3]+".ty"), pm.getAttr(list[3]+".tz"))
The 3D points are read from animated objects in the Maya scene. So at another frame,
I run this code to get
q0 = dt.Vector(pm.getAttr(list[0]+".tx"), pm.getAttr(list[0]+".ty"), pm.getAttr(list[0]+".tz"))
q1 = dt.Vector(pm.getAttr(list[1]+".tx"), pm.getAttr(list[1]+".ty"), pm.getAttr(list[1]+".tz"))
q2 = dt.Vector(pm.getAttr(list[2]+".tx"), pm.getAttr(list[2]+".ty"), pm.getAttr(list[2]+".tz"))
#q3 = TransformationMatrix between (p0,p1,p2) and (q0,q1,q2), applied to p3
I tried to calculate with vectors, but I ended up with errors due to divisions by zero...
So I figured that a transformation matrix should solve it without problems.
I've got a deadline not far ahead and I REALLY need help solving this!
PLEASE HELP!
Edit:
how to perform coordinates affine transformation using python?
I need this function "solve_affine", but it should take only 3 points from each set instead of 4. And I can't use numpy...
Here's a solution using numpy and scipy. scipy is mostly used to generate random rotations, except for scipy.linalg.norm which is easy to code oneself. The main things used from numpy are cross product and matrix multiplication, which are also easy to code oneself.
The basic idea is this: given three non-collinear points x1,x2,x3, it's possible to find an orthogonal triple of vectors (axes) v1,v2,v3, with v1 in the direction of x2-x1, v2 in the plane spanned by (x2-x1) and (x3-x1), and v3 completing the triple.
The points y1,y2,y3 are rotated and translated relative to x1,x2,x3. The axes w1,w2,w3 generated from y1,y2,y3 are rotated (i.e., no translation) from v1,v2,v3. These two sets of triples are each orthogonal, so it's easy to find the rotation from them: R = W * transpose(V)
Once we have the rotation, finding the translation is simple: y1 = R*x + t, so t = y1 - R*x. It might be a better to use a least-squares solver and combine all three points to get an estimate of t.
import numpy
import scipy.linalg
def rand_rot():
"""Return a random rotation
Return a random orthogonal matrix with determinant 1"""
q, _ = scipy.linalg.qr(numpy.random.randn(3, 3))
if scipy.linalg.det(q) < 0:
# does this ever happen?
print "got a negative det"
q[:, 0] = -q[:, 0]
return q
def rand_noncollinear():
"""Return 3 random non-collinear vectors"""
while True:
b = numpy.random.randn(3, 3)
sigma = scipy.linalg.svdvals(b)
if sigma[2]/sigma[0] > 0.1:
# "very" non-collinear
break
# "nearly" collinear; try again
return b[:, 0], b[:, 1], b[:, 2]
def normalize(a):
"""Return argument normalized"""
return a/scipy.linalg.norm(a)
def colstack(a1, a2, a3):
"""Stack three vectors as columns"""
return numpy.hstack((a1[:, numpy.newaxis],
a2[:, numpy.newaxis],
a3[:, numpy.newaxis]))
def get_axes(a1, a2, a3):
"""Generate orthogonal axes from three non-collinear points"""
# I tried to do this with QR, but something didn't work
b1 = normalize(a2-a1)
b2 = normalize(a3-a1)
b3 = normalize(numpy.cross(b1, b2))
b4 = normalize(numpy.cross(b3, b1))
return b1, b4, b3
# random rotation and translation
r = rand_rot()
t = numpy.random.randn(3)
# three non-collinear points
x1, x2, x3 = rand_noncollinear()
# some other point
x4 = numpy.random.randn(3)
# the images of the above in the transformation.
# y4 is for checking only -- won't be used to estimate r or t
y1, y2, y3, y4 = [numpy.dot(r, x) + t
for x in x1, x2, x3, x4]
v1, v2, v3 = get_axes(x1, x2, x3)
w1, w2, w3 = get_axes(y1, y2, y3)
V = colstack(v1, v2, v3)
W = colstack(w1, w2, w3)
# W = R V, so R = W * inverse(V); but V orthogonal, so inverse(V) is
# transpose(V):
rfound = numpy.dot(W, V.T)
# y1 = R x1 + t, so...
tfound = y1-numpy.dot(r, x1)
# get error on images of x2 and x3, just in case
y2err = scipy.linalg.norm(numpy.dot(rfound, x2) + tfound - y2)
y3err = scipy.linalg.norm(numpy.dot(rfound, x3) + tfound - y3)
# and check error image of x4 -- getting an estimate of y4 is the
# point of all of this
y4err = scipy.linalg.norm(numpy.dot(rfound, x4) + tfound - y4)
print "y2 error: ", y2err
print "y3 error: ", y3err
print "y4 error: ", y4err
Both the description and your code are confusing. Description is a bit vague while the code examples are missing important bits and pieces. So here is how I understand the question:
Knowing three points in two spaces how to construct a transform from space A to space B?
Image 1: How to form a transformation between 2 spaces.
The answer depends on the type of transform the spaces have. You see three points always form a planar span. This means that you can know what the rotation, transform, and uniform scale of the new space is. You can also know the shear on the plane, as well as nonuniform scale. However, you can not know what the shear or nonuniform scale would be in the plane normal direction.
Therefore to make sense the question mutates into how to rotate and translate two spaces to match? this is pretty easy to do Translation part is directly:
trans = q0 - p0
That leaves you with rotation which has been explained in several posts:
python + maya: Rotate Y axis to be along vector
How to convert three dimensional vector to an Euler rotation in software like Maya using python
You can also calculate a scaling factor after this.
I've figured it out
p0p1 = p1-p0
p0p2 = p2-p0
p0p3 = p3-p0
q0q1 = q1-q0
q0q2 = q2-q0
q0q3 = q3-q0
before = dt.Matrix([p0.x, p0.y, p0.z, 0],[p1.x, p1.y, p1.z, 0],[p2.x, p2.y, p2.z, 0], [0,0,0,1]);
after = dt.Matrix([q0.x, q0.y, q0.z, 0],[q1.x, q1.y, q1.z, 0],[q2.x, q2.y, q2.z, 0], [0,0,0,1]);
normal = p0p1.cross(p0p2).normal()
dist = p0p3.dot(normal)
q3 = p3 - dist*normal
transformMatrix = before.inverse()*after
solve = dt.Matrix(q3.x, q3.y, q3.z, 1)*transformMatrix
q3 = dt.Vector(solve[0][0], solve[0][1], solve[0][2])
newNormal = q0q1.cross(q0q2).normal()
q3 = q3 + newNormal*dist
pm.move(list[3], q3, r=False)
The transformation matrix only worked for points that are within the plane p0p1p2. So I solved it by transforming the projected point of p3, then move it out from the plane by the same distance.
If you have a solution that only involves a matrix, feel free to share, it may still help me! :)
I am trying to figure out the best way of calculating the volume of a 3D polyhedron with Python, and I'm hoping there is a simple solution out there, which I can't seem to find.
Example polyhedron
I did find this post that describes calculating the area of a planar polygon in 3D space, but that doesn't seem to help.
If you only have convex polyhedrons you can use the QHull binding of scipy.spatial.ConvexHull.
import numpy as np
from scipy.spatial import ConvexHull
points = np.array([[....]]) # your points
volume = ConvexHull(points).volume
Additionally, with the module Delaunay you can triangulate your passed points into tetrahedra for other stuff..
Is your polygon such that you can find a point inside so that you can connect every vertex to the point without crossing a face? If so, you can subdivide each face into triangles. You can do this easily by letting one vertex of a face be a pivot point and drawing lines from the other vertices to the pivot vertex. For instance, a pentagon gets divided into three triangles that fan from a common vertex. Each triangle will form a tetrahedron (a 3-sided pyramid) with the point inside. You can then add up the volumes of all of the tetrahedra for each face. The following is for a convex polyhedron that surrounds the origin (x=0,y=0,z=0). It assumes that there is a list of faces f, and each face has a list of vertices v.
def volume(self):
''' calculate volume of polyhedron '''
vol = 0.
for f in self.f: # the faces
n = len(f.v)
v2 = f.v[0] # the pivot of the fan
x2 = v2.x
y2 = v2.y
z2 = v2.z
for i in range(1,n-1): # divide into triangular fan segments
v0 = f.v[i]
x0 = v0.x
y0 = v0.y
z0 = v0.z
v1 = f.v[i+1]
x1 = v1.x
y1 = v1.y
z1 = v1.z
# Add volume of tetrahedron formed by triangle and origin
vol += math.fabs(x0 * y1 * z2 + x1 * y2 * z0 \
+ x2 * y0 * z1 - x0 * y2 * z1 \
- x1 * y0 * z2 - x2 * y1 * z0)
return vol/6.