pymeshio/common.py

   1 # coding: utf-8\r
   2 """\r
   3 common utilities.\r
   4 """\r
   5 import math\r
   6 import struct\r
   7 import sys\r
   8 import io\r
   9 \r
  10 \r
  11 def unicode(src):\r
  12     """\r
  13     literal to unicode for python2 and python3 compatiblity.\r
  14 \r
  15     in python2 str to unicode.\r
  16     in python3 str(as unicode) to str.\r
  17     """\r
  18     if sys.version_info[0]<3:\r
  19         return src.decode('utf-8')\r
  20     else:\r
  21         return src\r
  22 \r
  23 """\r
  24 common structures.\r
  25 """\r
  26 class Vector2(object):\r
  27     """\r
  28     2D coordinate for uv value\r
  29     """\r
  30     __slots__=['x', 'y']\r
  31     def __init__(self, x=0, y=0):\r
  32         self.x=x\r
  33         self.y=y\r
  34 \r
  35     def __str__(self):\r
  36         return "<%f %f>" % (self.x, self.y)\r
  37 \r
  38     def __eq__(self, rhs):\r
  39         return self.x==rhs.x and self.y==rhs.y\r
  40 \r
  41     def __ne__(self, rhs):\r
  42         return not self.__eq__(rhs)\r
  43 \r
  44     def __getitem__(self, key):\r
  45         if key==0:\r
  46             return self.x\r
  47         elif key==1:\r
  48             return self.y\r
  49         else:\r
  50             assert(False)\r
  51 \r
  52     def to_tuple(self):\r
  53         return (self.x, self.y)\r
  54 \r
  55     def cross(self, rhs):\r
  56         """cross(outer) product"""\r
  57         return self.x*rhs.y-self.y*rhs.x\r
  58 \r
  59 \r
  60 class Vector3(object):\r
  61     """\r
  62     3D coordinate for vertex position, normal direction\r
  63     """\r
  64     __slots__=['x', 'y', 'z']\r
  65     def __init__(self, x=0, y=0, z=0):\r
  66         self.x=x\r
  67         self.y=y\r
  68         self.z=z\r
  69 \r
  70     def __str__(self):\r
  71         return "<%f %.32f %f>" % (self.x, self.y, self.z)\r
  72 \r
  73     def __eq__(self, rhs):\r
  74         return self.x==rhs.x and self.y==rhs.y and self.z==rhs.z\r
  75 \r
  76     def __ne__(self, rhs):\r
  77         return not self.__eq__(rhs)\r
  78 \r
  79     def __getitem__(self, key):\r
  80         if key==0:\r
  81             return self.x\r
  82         elif key==1:\r
  83             return self.y\r
  84         elif key==2:\r
  85             return self.z\r
  86         else:\r
  87             assert(False)\r
  88 \r
  89     def to_tuple(self):\r
  90         return (self.x, self.y, self.z)\r
  91 \r
  92     def __add__(self, r):\r
  93         return Vector3(self.x+r.x, self.y+r.y, self.z+r.z)\r
  94 \r
  95     def __sub__(self, rhs):\r
  96         return Vector3(self.x-rhs.x, self.y-rhs.y, self.z-rhs.z)\r
  97 \r
  98     def getSqNorm(self):\r
  99         return self.x*self.x + self.y*self.y + self.z*self.z\r
 100 \r
 101     def getNorm(self):\r
 102         return math.sqrt(self.getSqNorm())\r
 103 \r
 104     def normalize(self):\r
 105         factor=1.0/self.getNorm()\r
 106         self.x*=factor\r
 107         self.y*=factor\r
 108         self.z*=factor\r
 109         return self\r
 110 \r
 111     def to_a(self):\r
 112         return [self.x, self.y, self.z]\r
 113 \r
 114     def dot(self, rhs):\r
 115         """dot(inner) product"""\r
 116         return self.x*rhs.x + self.y*rhs.y + self.z*rhs.z\r
 117 \r
 118     def cross(self, rhs):\r
 119         """cross(outer) product"""\r
 120         return Vector3(\r
 121                 self.y*rhs.z - rhs.y*self.z,\r
 122                 self.z*rhs.x - rhs.z*self.x,\r
 123                 self.x*rhs.y - rhs.x*self.y,\r
 124                 )\r
 125 \r
 126 \r
 127 class Quaternion(object):\r
 128     """\r
 129     rotation representation in vmd motion\r
 130     """\r
 131     __slots__=['x', 'y', 'z', 'w']\r
 132     def __init__(self, x=0, y=0, z=0, w=1):\r
 133         self.x=x\r
 134         self.y=y\r
 135         self.z=z\r
 136         self.w=w\r
 137 \r
 138     def __str__(self):\r
 139         return "<%f %f %f %f>" % (self.x, self.y, self.z, self.w)\r
 140 \r
 141     def __mul__(self, rhs):\r
 142         u=numpy.array([self.x, self.y, self.z], 'f')\r
 143         v=numpy.array([rhs.x, rhs.y, rhs.z], 'f')\r
 144         xyz=self.w*v+rhs.w*u+numpy.cross(u, v)\r
 145         q=Quaternion(xyz[0], xyz[1], xyz[2], self.w*rhs.w-numpy.dot(u, v))\r
 146         return q\r
 147 \r
 148     def dot(self, rhs):\r
 149         return self.x*rhs.x+self.y*rhs.y+self.z*rhs.z+self.w*rhs.w\r
 150 \r
 151     def getMatrix(self):\r
 152         sqX=self.x*self.x\r
 153         sqY=self.y*self.y\r
 154         sqZ=self.z*self.z\r
 155         xy=self.x*self.y\r
 156         xz=self.x*self.z\r
 157         yz=self.y*self.z\r
 158         wx=self.w*self.x\r
 159         wy=self.w*self.y\r
 160         wz=self.w*self.z\r
 161         return numpy.array([\r
 162                 # 1\r
 163                 [1-2*sqY-2*sqZ, 2*xy+2*wz, 2*xz-2*wy, 0],\r
 164                 # 2\r
 165                 [2*xy-2*wz, 1-2*sqX-2*sqZ, 2*yz+2*wx, 0],\r
 166                 # 3\r
 167                 [2*xz+2*wy, 2*yz-2*wx, 1-2*sqX-2*sqY, 0],\r
 168                 # 4\r
 169                 [0, 0, 0, 1]],\r
 170                 'f')\r
 171 \r
 172     def getRHMatrix(self):\r
 173         x=-self.x\r
 174         y=-self.y\r
 175         z=self.z\r
 176         w=self.w\r
 177         sqX=x*x\r
 178         sqY=y*y\r
 179         sqZ=z*z\r
 180         xy=x*y\r
 181         xz=x*z\r
 182         yz=y*z\r
 183         wx=w*x\r
 184         wy=w*y\r
 185         wz=w*z\r
 186         return numpy.array([\r
 187                 # 1\r
 188                 [1-2*sqY-2*sqZ, 2*xy+2*wz, 2*xz-2*wy, 0],\r
 189                 # 2\r
 190                 [2*xy-2*wz, 1-2*sqX-2*sqZ, 2*yz+2*wx, 0],\r
 191                 # 3\r
 192                 [2*xz+2*wy, 2*yz-2*wx, 1-2*sqX-2*sqY, 0],\r
 193                 # 4\r
 194                 [0, 0, 0, 1]],\r
 195                 'f')\r
 196 \r
 197     def getRollPitchYaw(self):\r
 198         m=self.getMatrix()\r
 199 \r
 200         roll = math.atan2(m[0, 1], m[1, 1])\r
 201         pitch = math.asin(-m[2, 1])\r
 202         yaw = math.atan2(m[2, 0], m[2, 2])\r
 203 \r
 204         if math.fabs(math.cos(pitch)) < 1.0e-6:\r
 205             roll += m[0, 1] > math.pi if 0.0 else -math.pi\r
 206             yaw += m[2, 0] > math.pi if 0.0 else -math.pi\r
 207 \r
 208         return roll, pitch, yaw\r
 209 \r
 210     def getSqNorm(self):\r
 211         return self.x*self.x+self.y*self.y+self.z*self.z+self.w*self.w\r
 212 \r
 213     def getNormalized(self):\r
 214         f=1.0/self.getSqNorm()\r
 215         q=Quaternion(self.x*f, self.y*f, self.z*f, self.w*f)\r
 216         return q\r
 217 \r
 218     def getRightHanded(self):\r
 219         "swap y and z axis"\r
 220         return Quaternion(-self.x, -self.z, -self.y, self.w)\r
 221 \r
 222     @staticmethod\r
 223     def createFromAxisAngle(axis, rad):\r
 224         q=Quaternion()\r
 225         half_rad=rad/2.0\r
 226         c=math.cos(half_rad)\r
 227         s=math.sin(half_rad)\r
 228         return Quaternion(axis[0]*s, axis[1]*s, axis[2]*s, c)\r
 229 \r
 230 \r
 231 class RGB(object):\r
 232     """\r
 233     material color\r
 234     """\r
 235     __slots__=['r', 'g', 'b']\r
 236     def __init__(self, r=0, g=0, b=0):\r
 237         self.r=r\r
 238         self.g=g\r
 239         self.b=b\r
 240 \r
 241     def __eq__(self, rhs):\r
 242         return self.r==rhs.r and self.g==rhs.g and self.b==rhs.b\r
 243 \r
 244     def __ne__(self, rhs):\r
 245         return not self.__eq__(rhs)\r
 246 \r
 247     def __getitem__(self, key):\r
 248         if key==0:\r
 249             return self.r\r
 250         elif key==1:\r
 251             return self.g\r
 252         elif key==2:\r
 253             return self.b\r
 254         else:\r
 255             assert(False)\r
 256 \r
 257 \r
 258 class RGBA(object):\r
 259     """\r
 260     material color\r
 261     """\r
 262     __slots__=['r', 'g', 'b', 'a']\r
 263     def __init__(self, r=0, g=0, b=0, a=1):\r
 264         self.r=r\r
 265         self.g=g\r
 266         self.b=b\r
 267         self.a=a\r
 268 \r
 269     def __eq__(self, rhs):\r
 270         return self.r==rhs.r and self.g==rhs.g and self.b==rhs.b and self.a==rhs.a\r
 271 \r
 272     def __ne__(self, rhs):\r
 273         return not self.__eq__(rhs)\r
 274 \r
 275     def __getitem__(self, key):\r
 276         if key==0:\r
 277             return self.r\r
 278         elif key==1:\r
 279             return self.g\r
 280         elif key==2:\r
 281             return self.b\r
 282         elif key==3:\r
 283             return self.a\r
 284         else:\r
 285             assert(False)\r
 286 \r
 287 \r
 288 """\r
 289 utilities\r
 290 """\r
 291 def radian_to_degree(x):\r
 292     """darian to deglee"""\r
 293 \r
 294     return x/math.pi * 180.0\r
 295 \r
 296 \r
 297 class ParseException(Exception):\r
 298     """\r
 299     Exception in reader\r
 300     """\r
 301     pass\r
 302 \r
 303 \r
 304 def readall(path):\r
 305     """read all bytes from path\r
 306     """\r
 307     with open(path, "rb") as f:\r
 308         return f.read()\r
 309 \r
 310 \r
 311 class BinaryReader(object):\r
 312     """general BinaryReader\r
 313     """\r
 314     def __init__(self, ios):\r
 315         current=ios.tell()\r
 316         ios.seek(0, io.SEEK_END)\r
 317         self.end=ios.tell()\r
 318         ios.seek(current)\r
 319         self.ios=ios\r
 320 \r
 321     def is_end(self):\r
 322         #print(self.ios.tell(), self.end)\r
 323         return self.ios.tell()>=self.end\r
 324         #return not self.ios.readable()\r
 325 \r
 326     def unpack(self, fmt, size):\r
 327         result=struct.unpack(fmt, self.ios.read(size))\r
 328         return result[0]\r
 329 \r
 330     def read_int(self, size):\r
 331         if size==1:\r
 332             return self.unpack("b", size)\r
 333         if size==2:\r
 334             return self.unpack("h", size)\r
 335         if size==4:\r
 336             return self.unpack("i", size)\r
 337         print("not reach here")\r
 338         raise ParseException("invalid int size: "+size)\r
 339 \r
 340     def read_uint(self, size):\r
 341         if size==1:\r
 342             return self.unpack("B", size)\r
 343         if size==2:\r
 344             return self.unpack("H", size)\r
 345         if size==4:\r
 346             return self.unpack("I", size)\r
 347         print("not reach here")\r
 348         raise ParseException("invalid int size: "+size)\r
 349 \r
 350     def read_float(self):\r
 351         return self.unpack("f", 4)\r
 352 \r
 353     def read_vector2(self):\r
 354         return Vector2(\r
 355                 self.read_float(), \r
 356                 self.read_float()\r
 357                 )\r
 358 \r
 359     def read_vector3(self):\r
 360         return Vector3(\r
 361                 self.read_float(), \r
 362                 self.read_float(), \r
 363                 self.read_float()\r
 364                 )\r
 365 \r
 366     def read_rgba(self):\r
 367         return RGBA(\r
 368                 self.read_float(), \r
 369                 self.read_float(), \r
 370                 self.read_float(),\r
 371                 self.read_float()\r
 372                 )\r
 373 \r
 374     def read_rgb(self):\r
 375         return RGB(\r
 376                 self.read_float(), \r
 377                 self.read_float(), \r
 378                 self.read_float()\r
 379                 )\r
 380 \r
 381 \r
 382 class WriteException(Exception):\r
 383     """\r
 384     Exception in writer\r
 385     """\r
 386     pass\r
 387 \r
 388 \r
 389 class BinaryWriter(object):\r
 390     def __init__(self, ios):\r
 391         self.ios=ios\r
 392 \r
 393     def write_bytes(self, v, size=None):\r
 394         if size:\r
 395             self.ios.write(struct.pack("={0}s".format(size), v))\r
 396         else:\r
 397             self.ios.write(v)\r
 398 \r
 399     def write_float(self, v):\r
 400         self.ios.write(struct.pack("f", v))\r
 401 \r
 402     def write_int(self, v, size):\r
 403         if size==1:\r
 404             self.ios.write(struct.pack("b", v))\r
 405         elif size==2:\r
 406             self.ios.write(struct.pack("h", v))\r
 407         elif size==4:\r
 408             self.ios.write(struct.pack("i", v))\r
 409         else:\r
 410             raise WriteError("invalid int uint size")\r
 411 \r
 412     def write_uint(self, v, size):\r
 413         if size==1:\r
 414             self.ios.write(struct.pack("B", v))\r
 415         elif size==2:\r
 416             self.ios.write(struct.pack("H", v))\r
 417         elif size==4:\r
 418             self.ios.write(struct.pack("I", v))\r
 419         else:\r
 420             raise WriteError("invalid int uint size")\r
 421 \r
 422     def write_vector2(self, v):\r
 423         self.ios.write(struct.pack("=2f", v.x, v.y))\r
 424 \r
 425     def write_vector3(self, v):\r
 426         self.ios.write(struct.pack("=3f", v.x, v.y, v.z))\r
 427 \r
 428     def write_rgb(self, v):\r
 429         self.ios.write(struct.pack("=3f", v.r, v.g, v.b))\r
 430 \r
 431     def write_rgba(self, v):\r
 432         self.ios.write(struct.pack("=4f", v.r, v.g, v.b, v.a))\r
 433 \r
 434 \r