rickthepizza / pyeuclid

Automatically exported from code.google.com/p/pyeuclid
0 stars 0 forks source link

Vector Rotation (3D) and Angle Between Functions #10

Closed GoogleCodeExporter closed 9 years ago

GoogleCodeExporter commented 9 years ago
Here's a couple functions I've used to rotate a vector about another vector and 
find the angle between two vectors  in 3 dimensions:

from math import sqrt, sin, cos, acos

def rotate(v1, v2, theta):
    """Rotate vector v1 about v2 by the angle theta in radians. The right hand rule applies."""

    # Adapted from equations published by Glenn Murray - Thanks Glenn!!!
    # http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ArbitraryAxisRotation.html
    x, y, z = v1
    u, v, w = v2
    newx = (
            (
                u*(u*x + v*y + w*z)
                + (x*(v**2 + w**2) + u*(-v*y - w*z))*cos(theta)
                + sqrt(u**2 + v**2 + w**2)*(-w*y + v*z)*sin(theta)
            )
            / (u**2 + v**2 + w**2)
        )
    newy = (
            (
                v*(u*x + v*y + w*z)
                + (y*(u**2 + w**2) + v*(-u*x - w*z))*cos(theta)
                + sqrt(u**2 + v**2 + w**2)*(w*x - u*z)*sin(theta)
            )
            / (u**2 + v**2 + w**2)
        )
    newz = (
            (
                w*(u*x + v*y + w*z)
                + (z*(u**2 + v**2) + w*(-u*x - v*y))*cos(theta)
                + sqrt(u**2 + v**2 + w**2)*(-v*x + u*y)*sin(theta)
            )
            / (u**2 + v**2 + w**2)
        )
    return vector([newx,newy,newz])

def angle(v1, v2):
    """Calculate the angle between vectors v1 and v2 => radians"""
    return acos(dotproduct(v1, v2) / (v1.mag*v2.mag))

These are from my own vector class, but should drop into yours without much 
trouble. Anyways, thought you might be interested. You can find the whole class 
at http://thefekete.net/gitweb/?p=python/robotarm.git;a=blob;f=pyVec.py

BTW, I'm new to google code and couldn't find a way to message the maintainers 
or commit a patch, so I'm dumping this here...

-dan

Original issue reported on code.google.com by TheFek...@gmail.com on 9 Dec 2010 at 5:44

GoogleCodeExporter commented 9 years ago
I just added the methods Vector3.rotate_around() and Vector2.angle() and 
Vector3.angle() that provide this functionality.

Original comment by dov.grob...@gmail.com on 5 Apr 2011 at 7:14