viewODE/vecMath.py at master · gedeschaines/viewODE · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
# vecMath.py
#
# Vector math functions module.
#
# Originally by Gary Deschaines, 2009
#
# Attributions
#
# + Matt Heinzen for PyODE Ragdoll Physics Tutorial program ragdoll-pyode-tutorial.py
#   available at http://monsterden.net/software/ragdoll-pyode-tutorial which was used
#   as a basis for the vector math functions herein.
#
# Disclaimers
#
#   See the file DISCLAIMER-GaryDeschaines
#   See the file DISCLAIMER-MattHeinzen

from math import isinf, isnan, pi, sqrt, acos, atan2

# Math Conversion Factors and Constants

RPD = pi / 180.0
DPR = 180.0 / pi
TWOPI = 2.0 * pi
HALFPI = pi / 2.0
INFINITY = float("inf")


# Vector Math

def vecMulS(V, s):
    return (V[0] * s, V[1] * s, V[2] * s)


def vecDivS(V, s):
    if s == 0.0:
        return (INFINITY, INFINITY, INFINITY)
    else:
        return (float(V[0]) / s, float(V[1]) / s, float(V[2]) / s)


def vecAdd(A, B):
    return (A[0] + B[0], A[1] + B[1], A[2] + B[2])


def vecSub(A, B):
    return (A[0] - B[0], A[1] - B[1], A[2] - B[2])


def vecMagSq(V):
    if True not in [isnan(V[i]) for i in range(len(V))]:
        try:
            magsq = V[0] ** 2 + V[1] ** 2 + V[2] ** 2
        except OverflowError:
            magsq = INFINITY
    else:
        magsq = INFINITY
    return magsq


def vecMag(V):
    magsq = vecMagSq(V)
    if isinf(magsq):
        return INFINITY
    if magsq > 0.0:
        return sqrt(magsq)
    else:
        return 0.0


def unitVec(V):
    mag = vecMag(V)
    if isinf(mag):
        return (0.0, 0.0, 0.0)
    if mag > 0.0:
        return vecMulS(V, 1.0 / mag)
    else:
        return (0.0, 0.0, 0.0)


def vecDotP(A, B):
    return (A[0] * B[0] + A[1] * B[1] + A[2] * B[2])


def acosUVecDotP(UA, UB):
    cosang = vecDotP(UA, UB)
    if cosang < -1.0:
        return pi
    elif cosang > 1.0:
        return 0.0
    else:
        return acos(cosang)


def acosVecDotP(A, B):
    UA = unitVec(A)
    UB = unitVec(B)
    return acosUVecDotP(UA, UB)


def projectVecAonUB(A, UB):
    return vecMulS(UB, vecDotP(A, UB))


def rejectionVecAfromUB(A, UB):
    return vecSub(A, projectVecAonUB(A, UB))


def vecCrossP(A, B):
    return (A[1] * B[2] - B[1] * A[2],
            A[2] * B[0] - B[2] * A[0],
            A[0] * B[1] - B[0] * A[1])


def atanVecCrossP(A, B):
    x = vecDotP(A, B)
    y = vecMag(vecCrossP(A, B))
    return atan2(y, x)


def unitNormalVecFromAtoB(A, B):
    return unitVec(vecCrossP(A, B))


def unitNormalVecAndAngleFromAtoB(A, B):
    Nvec = vecCrossP(A, B)
    return (unitVec(Nvec), atan2(vecMag(Nvec), vecDotP(A, B)))