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