eic-opticks/analytic/glm.py

0001 #!/usr/bin/env python
0002 #
0003 # Copyright (c) 2019 Opticks Team. All Rights Reserved.
0004 #
0005 # This file is part of Opticks
0006 # (see https://bitbucket.org/simoncblyth/opticks).
0007 #
0008 # Licensed under the Apache License, Version 2.0 (the "License");
0009 # you may not use this file except in compliance with the License.
0010 # You may obtain a copy of the License at
0011 #
0012 #   http://www.apache.org/licenses/LICENSE-2.0
0013 #
0014 # Unless required by applicable law or agreed to in writing, software
0015 # distributed under the License is distributed on an "AS IS" BASIS,
0016 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
0017 # See the License for the specific language governing permissions and
0018 # limitations under the License.
0019 #
0020
0021 """
0022 """
0023 import logging
0024 log = logging.getLogger(__name__)
0025 import numpy as np
0026 fromstring_  = lambda s:np.fromstring(s, dtype=np.float32, sep=",")
0027
0028
0029 def to_list(a, fmt="%.3f", lang="py"):
0030     s = ",".join(map(lambda _:fmt % _, list(a) ))
0031     fmt = "[%s]" if lang == "py" else "%s"
0032     return fmt % s
0033
0034 def to_pylist(a, fmt="%.3f"):
0035     return to_list(a, fmt, lang="py")
0036
0037 def to_cpplist(a, fmt="%.3f"):
0038     return to_list(a, fmt, lang="cpp")
0039
0040
0041 def to_pyline(a, alabel="a", fmt="%.3f", lang="py"):
0042     if a is None:
0043         s = "None" if lang == "py" else "NULL"
0044     elif a.shape == (4,):
0045         s = to_list(a, fmt, lang=lang)
0046     elif a.shape == (3,):
0047         s = to_list(a, fmt, lang=lang)
0048     elif a.shape == (4,4):
0049         ss = map(to_pylist if lang=="py" else to_cpplist, a )
0050         if lang == "py":
0051             s = "[" + ",".join(ss) + "]"
0052         else:
0053             s = "nmat4triple::make_transform(" + ",  ".join(ss) + ") ;"
0054         pass
0055     else:
0056         pass
0057     pass
0058     return "%s" % s if alabel is None else "%s = %s" % (alabel, s)
0059
0060
0061 def to_codeline(a, alabel="a", fmt="%.3f", lang="py"):
0062     return to_pyline(a, alabel, fmt, lang=lang)
0063
0064
0065
0066
0067
0068
0069 def dot_(trs, reverse=False, left=False):
0070     dr = dot_reduce(trs, reverse=reverse)
0071     dl = dot_loop(trs, reverse=reverse)
0072     assert np.allclose(dr,dl)
0073     return dl
0074
0075 def mdot_(args):
0076     """
0077     * http://scipy-cookbook.readthedocs.io/items/MultiDot.html
0078
0079     This will always give you left to right associativity, i.e. the expression is interpreted as `(((a*b)*c)*d)`.
0080
0081     """
0082     ret = args[0]
0083     for a in args[1:]:
0084         ret = np.dot(ret,a)
0085     pass
0086
0087     chk = reduce(np.dot, args)
0088     assert np.allclose(chk,ret)
0089
0090     return ret
0091
0092 def mdotr_(args):
0093     """
0094     * http://scipy-cookbook.readthedocs.io/items/MultiDot.html
0095
0096     right-associative version of the loop: which evaluates as `(a*(b*(c*d)))`
0097
0098     """
0099     ret = args[-1]
0100     for a in reversed(args[:-1]):
0101         ret = np.dot(a,ret)
0102     pass
0103     return ret
0104
0105
0106 def dot_loop(trs, reverse=False, left=False):
0107     trs_ = trs if not reverse else trs[::-1]
0108     m = np.eye(4)
0109     for tr in trs_:
0110         if left:
0111             m = np.dot(tr, m)
0112         else:
0113             m = np.dot(m, tr)
0114         pass
0115     pass
0116     return np.array(m, dtype=np.float32)
0117
0118
0119 def dot_reduce(trs, reverse=False):
0120     """
0121
0122     http://scipy-cookbook.readthedocs.io/items/MultiDot.html
0123
0124     Suspicious of the reduce multiplicaton order
0125
0126     ::
0127
0128         In [6]: reduce(np.dot, nd.trs)
0129         Out[6]:
0130         array([[      0.5432,      -0.8396,       0.    ,       0.    ],
0131                [      0.8396,       0.5432,       0.    ,       0.    ],
0132                [      0.    ,       0.    ,       1.    ,       0.    ],
0133                [  19391.    ,  802110.    ,   -7100.    ,       1.    ]], dtype=float32)
0134
0135         In [7]: reduce(np.dot, nd.trs[::-1])
0136         Out[7]:
0137         array([[      0.5432,      -0.8396,       0.    ,       0.    ],
0138                [      0.8396,       0.5432,       0.    ,       0.    ],
0139                [      0.    ,       0.    ,       1.    ,       0.    ],
0140                [ -18079.4531, -799699.4375,   -7100.    ,       1.    ]], dtype=float32)
0141
0142     """
0143     trs_ = trs if not reverse else trs[::-1]
0144     return reduce(np.dot, trs_ )
0145
0146
0147
0148 def make_scale(arg=[1,2,3], m=None, dtype=np.float32):
0149     """
0150     Translation of glm::scale into numpy
0151     /usr/local/opticks/externals/glm/glm-0.9.6.3/glm/gtc/matrix_transform.inl
0152     """
0153     if m is None:m = np.eye(4, dtype=dtype)
0154
0155     if type(arg) is str:
0156         arg = fromstring_(arg)
0157         assert arg.shape == (3,)
0158     elif arg is None:
0159         arg = [1,1,1]
0160     else:
0161         pass
0162     pass
0163     v = np.asarray(arg, dtype=dtype)
0164
0165     Result = np.eye(4, dtype=dtype)
0166     Result[0] = m[0] * v[0];
0167     Result[1] = m[1] * v[1];
0168     Result[2] = m[2] * v[2];
0169     Result[3] = m[3];
0170     return Result
0171
0172 def translate(arg=[1,2,3], m=None, dtype=np.float32):
0173     """
0174     Translation of glm::translate into numpy
0175     /usr/local/opticks/externals/glm/glm-0.9.6.3/glm/gtc/matrix_transform.inl
0176     """
0177     if type(arg) is str:
0178         arg = fromstring_(arg)
0179         assert arg.shape == (3,)
0180     elif arg is None:
0181         arg = [0,0,0]
0182     else:
0183         pass
0184     pass
0185     v = np.asarray(arg, dtype=dtype)
0186
0187     if m is None:m = np.eye(4, dtype=dtype)
0188     Result = np.eye(4, dtype=dtype)
0189     Result[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]
0190     return Result
0191
0192
0193 def rotate_three_axis(arg=[0,0,90], m=None, dtype=np.float32, transpose=False):
0194     """
0195     :param arg: 3-component array
0196     """
0197     if m is None:m = np.eye(4, dtype=dtype)
0198
0199     if arg is not None:
0200         assert len(arg) == 3, arg
0201         rx = arg[0]
0202         ry = arg[1]
0203         rz = arg[2]
0204
0205         if rx != 0: m = rotate([1,0,0,rx], m, transpose=transpose )
0206         if ry != 0: m = rotate([0,1,0,ry], m, transpose=transpose )
0207         if rz != 0: m = rotate([0,0,1,rz], m, transpose=transpose )
0208     pass
0209
0210     return m
0211
0212
0213
0214
0215
0216 def rotate(arg=[0,0,1,45], m=None, dtype=np.float32, transpose=False):
0217     """
0218     :param arg: 4-component array, 1st three for axis, 4th for rotation angle in degrees
0219     :param m: optional matrix to combine with
0220
0221     Translation of glm::rotate into numpy
0222     /usr/local/opticks/externals/glm/glm-0.9.6.3/glm/gtc/matrix_transform.inl::
0223     """
0224     if m is None:m = np.eye(4, dtype=dtype)
0225
0226     if type(arg) is str:
0227         arg = fromstring_(arg)
0228         assert arg.shape == (4,)
0229     elif arg is None:
0230         arg = [0,0,1,0]
0231     else:
0232         pass
0233     pass
0234     v = np.asarray(arg, dtype=dtype)
0235
0236     axis_ = v[:3]
0237     angle_d = v[3]
0238
0239     axis = np.array( axis_, dtype=dtype)
0240     axis /= np.sqrt(np.dot(axis,axis))
0241
0242     angle = np.pi*float(angle_d)/180.
0243     c = np.cos(angle)
0244     s = np.sin(angle)
0245
0246     temp = (1. - c)*axis
0247
0248     Rotate  = np.eye(3, dtype=dtype)
0249     Rotate[0][0] = c + temp[0] * axis[0]
0250     Rotate[0][1] = 0 + temp[0] * axis[1] + s * axis[2]
0251     Rotate[0][2] = 0 + temp[0] * axis[2] - s * axis[1]
0252
0253     Rotate[1][0] = 0 + temp[1] * axis[0] - s * axis[2]
0254     Rotate[1][1] = c + temp[1] * axis[1]
0255     Rotate[1][2] = 0 + temp[1] * axis[2] + s * axis[0]
0256
0257     Rotate[2][0] = 0 + temp[2] * axis[0] + s * axis[1]
0258     Rotate[2][1] = 0 + temp[2] * axis[1] - s * axis[0]
0259     Rotate[2][2] = c + temp[2] * axis[2]
0260
0261     if transpose:
0262         log.debug("adhoc transposing rotation")
0263         Rotate = Rotate.T   #  <--- huh, adhoc addition to to get PMTs to point in correct direction
0264     pass
0265
0266     Result = np.eye(4, dtype=dtype)
0267     Result[0] = m[0] * Rotate[0][0] + m[1] * Rotate[0][1] + m[2] * Rotate[0][2];
0268     Result[1] = m[0] * Rotate[1][0] + m[1] * Rotate[1][1] + m[2] * Rotate[1][2];
0269     Result[2] = m[0] * Rotate[2][0] + m[1] * Rotate[2][1] + m[2] * Rotate[2][2];
0270     Result[3] = m[3];
0271
0272     return Result;
0273
0274
0275
0276
0277 def make_transform( order, tla, rot, sca, dtype=np.float32, suppress_identity=True, three_axis_rotate=False, transpose_rotation=False):
0278     """
0279     :param order: string containing "s" "r" and "t", standard order is "trs" meaning t*r*s  ie scale first, then rotate, then translate
0280     :param tla: tx,ty,tz tranlation dists eg 0,0,0 for no translation
0281     :param rot: ax,ay,az,angle_degrees  eg 0,0,1,45 for 45 degrees about z-axis
0282     :param sca: sx,sy,sz eg 1,1,1 for no scaling
0283     :return mat: 4x4 numpy array
0284
0285     All arguments can be specified as comma delimited string, list or numpy array
0286
0287     Translation of npy/tests/NGLMTest.cc:make_mat
0288     """
0289
0290     if tla is None and rot is None and sca is None and suppress_identity:
0291         return None
0292
0293     identity = np.eye(4, dtype=dtype)
0294     m = np.eye(4, dtype=dtype)
0295     for c in order:
0296         if c == 's':
0297             m = make_scale(sca, m)
0298         elif c == 'r':
0299             if three_axis_rotate:
0300                 m = rotate_three_axis(rot, m, transpose=transpose_rotation )
0301             else:
0302                 m = rotate(rot, m, transpose=transpose_rotation )
0303             pass
0304         elif c == 't':
0305             m = translate(tla, m)
0306         else:
0307             assert 0
0308         pass
0309     pass
0310
0311     if suppress_identity and np.all( m == identity ):
0312         #log.warning("supressing identity transform")
0313         return None
0314     pass
0315     return m
0316
0317
0318
0319 def make_trs( tla, rot, sca, three_axis_rotate=False, dtype=np.float32):
0320     return make_transform("trs", tla, rot, sca, three_axis_rotate=three_axis_rotate, dtype=dtype )
0321
0322
0323 def test_make_transform():
0324     """
0325     compare with npy/tests/NGLMTest.cc:test_make_transform
0326     """
0327     fromstr = True
0328     if fromstr:
0329         tla = "0,0,100"
0330         rot = "0,0,1,45"
0331         sca = "1,2,3"
0332     else:
0333         tla = [0,0,100]
0334         rot = [0,0,1,45]
0335         sca = [1,2,3]
0336     pass
0337
0338     dtype = np.float32
0339     t = make_transform("t", tla, rot, sca, dtype=dtype )
0340     assert(t.dtype == dtype)
0341
0342     r = make_transform("r", tla, rot, sca, dtype=dtype)
0343     assert(t.dtype == dtype)
0344
0345     s = make_transform("s", tla, rot, sca, dtype=dtype)
0346     assert(s.dtype == dtype)
0347
0348     trs = make_transform("trs", tla, rot, sca, dtype=dtype)
0349     assert(trs.dtype == dtype)
0350
0351     trs2 = make_trs(tla, rot, sca, dtype=dtype)
0352     assert(trs2.dtype == dtype )
0353
0354
0355     print("t\n%s" % t)
0356     print("r\n%s" % r)
0357     print("s\n%s" % s)
0358     print("trs\n%s" % trs)
0359
0360
0361
0362
0363 if __name__ == '__main__':
0364
0365     rot = rotate([0,0,1,45])
0366     print("rot\n%s" % rot)
0367
0368     sca = make_scale([1,2,3])
0369     print("sca\n%s" % sca)
0370
0371     test_make_transform()
0372
0373     rot3x = rotate_three_axis([45,0,0])
0374     rot3y = rotate_three_axis([0,45,0])
0375     rot3z = rotate_three_axis([0,0,45])
0376
0377     rot3xyz = rotate_three_axis([45,45,45])
0378
0379
0380     print("rot3x\n%s" % rot3x)
0381     print("rot3y\n%s" % rot3y)
0382     print("rot3z\n%s" % rot3z)
0383
0384     print("rot3xyz\n%s" % rot3xyz)
0385
0386
0387     a = np.array([1,2,3,4],dtype=np.float32)
0388     m = np.eye(4, dtype=np.float32)
0389
0390     print(to_pyline(a,"a"))
0391     print(to_pyline(m,"m"))
0392
0393
0394
0395