Add many defuns for points and vectors

diff --git a/math.lisp b/math.lisp
index c3b3beaad4a7c6d798de28fb9174817a1706dca3..4ac87942988cc245162595172182bca8e2d4cc52 100644 (file)
--- a/math.lisp
+++ b/math.lisp
@@ -7,3 +7,80 @@
 (defmacro inv (x) `(/ (float ,x)))
 
 (defmacro E (number exponent) `(* (float ,number) (^ 10 ,exponent)))
+
+(defun deg->rad (degrees)
+  "Convert to radians from degrees."
+  (/ (* degrees pi) 180))
+
+(defun rad->deg (radians)
+  "Convert to degrees from radians."
+  (/ (* radians 180) pi))
+
+(defun average-of-points (points)
+  "Calculate an average of 2d or 3d points, given as a list of lists.
+If a 2d point is given, it is converted to 3d with z=0."
+  (cond ((numberp (first points))
+        points)
+       ((= (length points) 1)
+        (first points))
+       (t (mapcar #'/
+                  (list (apply #'+ (mapcar #'first points))
+                        (apply #'+ (mapcar #'second points))
+                        (apply #'+
+                               (mapcar #'(lambda (x)
+                                           (if (null (third x))
+                                               0
+                                               (third x)))
+                                       points)))
+                  '(2 2 2)))))
+
+(defun dot-product-points (apoint bpoint)
+  "Return the dot-product of 3d vectors a and b.
+Inputs are of the form '(ax ay az) '(bx by bz).
+The function returns a number."
+  (apply #'+ (mapcar #'* apoint bpoint)))
+
+(defun vector-magnitude (endpoint)
+  "Calculate the vector's magnitude, given an endpoint.
+Endpoint is assumed relative to origin."
+  (sqrt (apply #'+ (mapcar #'(lambda (x) (square x)) endpoint))))
+
+(defun angle-between-vectors (apoint bpoint)
+  "Retun the angle (rads) between two vectors.
+Inputs are of the form '(ax ay az) '(bx by bz).
+The function returns a number."
+  (acos (/ (dot-product-points apoint bpoint)
+          (* (vector-magnitude apoint)
+             (vector-magnitude bpoint)))))
+
+(defun vector-endpoints (vector)
+  "Calculate a 3d vector from the magnitude and angle (rads) of the same.
+Z coordinate will always be 0 in the returned value."
+  (let ((mag (first vector))
+       (theta (second vector)))
+    (list (* mag (cos theta)) (* mag (sin theta)) 0)))
+
+(defun cross-product-points (apoint bpoint)
+  "Return a list of the endpoint of the cross-product vector c.
+Inputs are of the form '(ax ay az) '(bx by bz).
+ If 2d vectors are given, it is converted to 3d with z=0.
+The function returns '(cx cy cz)."
+  (let ((ax (first apoint))
+       (ay (second apoint))
+       (az (if (null (third apoint))
+               0
+               (third apoint)))
+       (bx (first bpoint))
+       (by (second bpoint))
+       (bz (if (null (third bpoint))
+               0
+               (third bpoint))))
+    (list (- (* ay bz) (* az by))
+         (- (* az bx) (* ax bz))
+         (- (* ax by) (* ay bx)))))
+
+(defun cross-product-vectors (avec bvec)
+  "Return a list representing the cross-product vector c.
+Inputs are of the form '(amag atheta) '(bmag btheta), with theta in rads.
+The function returns '(cx cy cz), and assumes both inputs are 2d."
+  (cross-product-points (vector-endpoints avec) (vector-endpoints bvec)))