Advertisement
Guest User

Untitled

a guest
Jun 26th, 2019
96
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.58 KB | None | 0 0
  1. x^2 +h^2 = r1^2
  2. (d-x)^2 +h^2 = r2^2
  3. ==> h = sqrt(r1^2 - 1/d^2*(r1^2-r2^2+d^2)^2)
  4.  
  5. X = C + (h * cos t) U + (h * sin t) V for t in [0,2*PI)
  6.  
  7. import numpy
  8. from numpy import sqrt, dot, cross
  9. from numpy.linalg import norm
  10.  
  11. # Find the intersection of three spheres
  12. # P1,P2,P3 are the centers, r1,r2,r3 are the radii
  13. # Implementaton based on Wikipedia Trilateration article.
  14. def trilaterate(P1,P2,P3,r1,r2,r3):
  15. temp1 = P2-P1
  16. e_x = temp1/norm(temp1)
  17. temp2 = P3-P1
  18. i = dot(e_x,temp2)
  19. temp3 = temp2 - i*e_x
  20. e_y = temp3/norm(temp3)
  21. e_z = cross(e_x,e_y)
  22. d = norm(P2-P1)
  23. j = dot(e_y,temp2)
  24. x = (r1*r1 - r2*r2 + d*d) / (2*d)
  25. y = (r1*r1 - r3*r3 -2*i*x + i*i + j*j) / (2*j)
  26. temp4 = r1*r1 - x*x - y*y
  27. if temp4<0:
  28. raise Exception("The three spheres do not intersect!");
  29. z = sqrt(temp4)
  30. p_12_a = P1 + x*e_x + y*e_y + z*e_z
  31. p_12_b = P1 + x*e_x + y*e_y - z*e_z
  32. return p_12_a,p_12_b
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement