text: #################################################################
  # seGeometry.py
  # Originally from DirectGeometry.py
  # Altered by Yi-Hong Lin, yihhongl@andrew.cmu.edu, 2004
  #
  # We didn't change anything essential.
  # Just because we customized the seSession from DirectSession,
  # So we need related files can follow the change.
  # However, we don't want to change anything inside the original directool
  # to let them can work with our scene editor.
  # (If we do change original directools, it will force user has to install the latest version of OUR Panda)
  #
  #################################################################
  
  from panda3d.core import *
  from direct.directtools.DirectGlobals import *
  from direct.directtools.DirectUtil import *
  import math
  
  class LineNodePath(NodePath):
      def __init__(self, parent = None, name = None,
                   thickness = 1.0, colorVec = VBase4(1)):
  
          # Initialize the superclass
          NodePath.__init__(self)
  
          if parent is None:
              parent = hidden
  
          # Attach a geomNode to the parent and set self to be
          # the resulting node path
          self.lineNode = GeomNode("lineNode")
          self.assign(parent.attachNewNode( self.lineNode ))
          if name:
              self.setName(name)
  
          # Create a lineSegs object to hold the line
          ls = self.lineSegs = LineSegs()
          # Initialize the lineSegs parameters
          ls.setThickness(thickness)
          ls.setColor(colorVec)
  
      def moveTo( self, *_args ):
          self.lineSegs.moveTo(*_args)
  
      def drawTo( self, *_args ):
          self.lineSegs.drawTo(*_args)
  
      def create( self, frameAccurate = 0 ):
          self.lineSegs.create( self.lineNode, frameAccurate )
  
      def reset( self ):
          self.lineSegs.reset()
          self.lineNode.removeAllGeoms()
  
      def isEmpty( self ):
          return self.lineSegs.isEmpty()
  
      def setThickness( self, thickness ):
          self.lineSegs.setThickness( thickness )
  
      def setColor( self, *_args ):
          self.lineSegs.setColor(*_args)
  
      def setVertex( self, *_args):
          self.lineSegs.setVertex(*_args)
  
      def setVertexColor( self, vertex, *_args ):
          self.lineSegs.setVertexColor(*(vertex,) + _args)
  
      def getCurrentPosition( self ):
          return self.lineSegs.getCurrentPosition()
  
      def getNumVertices( self ):
          return self.lineSegs.getNumVertices()
  
      def getVertex( self, index ):
          return self.lineSegs.getVertex(index)
  
      def getVertexColor( self ):
          return self.lineSegs.getVertexColor()
  
      def drawArrow(self, sv, ev, arrowAngle, arrowLength):
          """
          Do the work of moving the cursor around to draw an arrow from
          sv to ev. Hack: the arrows take the z value of the end point
          """
          self.moveTo(sv)
          self.drawTo(ev)
          v = sv - ev
          # Find the angle of the line
          angle = math.atan2(v[1], v[0])
          # Get the arrow angles
          a1 = angle + deg2Rad(arrowAngle)
          a2 = angle - deg2Rad(arrowAngle)
          # Get the arrow points
          a1x = arrowLength * math.cos(a1)
          a1y = arrowLength * math.sin(a1)
          a2x = arrowLength * math.cos(a2)
          a2y = arrowLength * math.sin(a2)
          z = ev[2]
          self.moveTo(ev)
          self.drawTo(Point3(ev + Point3(a1x, a1y, z)))
          self.moveTo(ev)
          self.drawTo(Point3(ev + Point3(a2x, a2y, z)))
  
      def drawArrow2d(self, sv, ev, arrowAngle, arrowLength):
          """
          Do the work of moving the cursor around to draw an arrow from
          sv to ev. Hack: the arrows take the z value of the end point
          """
          self.moveTo(sv)
          self.drawTo(ev)
          v = sv - ev
          # Find the angle of the line
          angle = math.atan2(v[2], v[0])
          # Get the arrow angles
          a1 = angle + deg2Rad(arrowAngle)
          a2 = angle - deg2Rad(arrowAngle)
          # Get the arrow points
          a1x = arrowLength * math.cos(a1)
          a1y = arrowLength * math.sin(a1)
          a2x = arrowLength * math.cos(a2)
          a2y = arrowLength * math.sin(a2)
          self.moveTo(ev)
          self.drawTo(Point3(ev + Point3(a1x, 0.0, a1y)))
          self.moveTo(ev)
          self.drawTo(Point3(ev + Point3(a2x, 0.0, a2y)))
  
      def drawLines(self, lineList):
          """
          Given a list of lists of points, draw a separate line for each list
          """
          for pointList in lineList:
              self.moveTo(*pointList[0])
              for point in pointList[1:]:
                  self.drawTo(*point)
  
  ##
  ## Given a point in space, and a direction, find the point of intersection
  ## of that ray with a plane at the specified origin, with the specified normal
  def planeIntersect (lineOrigin, lineDir, planeOrigin, normal):
      t = 0
      offset = planeOrigin - lineOrigin
      t = offset.dot(normal) / lineDir.dot(normal)
      hitPt = lineDir * t
      return hitPt + lineOrigin
  
  def getNearProjectionPoint(nodePath):
      # Find the position of the projection of the specified node path
      # on the near plane
      origin = nodePath.getPos(SEditor.camera)
      # project this onto near plane
      if origin[1] != 0.0:
          return origin * (SEditor.dr.near / origin[1])
      else:
          # Object is coplaner with camera, just return something reasonable
          return Point3(0, SEditor.dr.near, 0)
  
  def getScreenXY(nodePath):
      # Where does the node path's projection fall on the near plane
      nearVec = getNearProjectionPoint(nodePath)
      # Clamp these coordinates to visible screen
      nearX = CLAMP(nearVec[0], SEditor.dr.left, SEditor.dr.right)
      nearY = CLAMP(nearVec[2], SEditor.dr.bottom, SEditor.dr.top)
      # What percentage of the distance across the screen is this?
      percentX = (nearX - SEditor.dr.left)/SEditor.dr.nearWidth
      percentY = (nearY - SEditor.dr.bottom)/SEditor.dr.nearHeight
      # Map this percentage to the same -1 to 1 space as the mouse
      screenXY = Vec3((2 * percentX) - 1.0,nearVec[1],(2 * percentY) - 1.0)
      # Return the resulting value
      return screenXY
  
  def getCrankAngle(center):
      # Used to compute current angle of mouse (relative to the coa's
      # origin) in screen space
      x = SEditor.dr.mouseX - center[0]
      y = SEditor.dr.mouseY - center[2]
      return (180 + rad2Deg(math.atan2(y,x)))
  
  def relHpr(nodePath, base, h, p, r):
      # Compute nodePath2newNodePath relative to base coordinate system
      # nodePath2base
      mNodePath2Base = nodePath.getMat(base)
      # delta scale, orientation, and position matrix
      mBase2NewBase = Mat4()
      composeMatrix(mBase2NewBase, UNIT_VEC, VBase3(h,p,r), ZERO_VEC,
                    CSDefault)
      # base2nodePath
      mBase2NodePath = base.getMat(nodePath)
      # nodePath2 Parent
      mNodePath2Parent = nodePath.getMat()
      # Compose the result
      resultMat = mNodePath2Base * mBase2NewBase
      resultMat = resultMat * mBase2NodePath
      resultMat = resultMat * mNodePath2Parent
      # Extract and apply the hpr
      hpr = Vec3(0)
      decomposeMatrix(resultMat, VBase3(), hpr, VBase3(),
                      CSDefault)
      nodePath.setHpr(hpr)
  
  # Quaternion interpolation
  def qSlerp(startQuat, endQuat, t):
      startQ = Quat(startQuat)
      destQuat = Quat.identQuat()
      # Calc dot product
      cosOmega = (startQ.getI() * endQuat.getI() +
                  startQ.getJ() * endQuat.getJ() +
                  startQ.getK() * endQuat.getK() +
                  startQ.getR() * endQuat.getR())
      # If the above dot product is negative, it would be better to
      # go between the negative of the initial and the final, so that
      # we take the shorter path.
      if ( cosOmega < 0.0 ):
          cosOmega *= -1
          startQ.setI(-1 * startQ.getI())
          startQ.setJ(-1 * startQ.getJ())
          startQ.setK(-1 * startQ.getK())
          startQ.setR(-1 * startQ.getR())
      if ((1.0 + cosOmega) > Q_EPSILON):
          # usual case
          if ((1.0 - cosOmega) > Q_EPSILON):
              # usual case
              omega = math.acos(cosOmega)
              sinOmega = math.sin(omega)
              startScale = math.sin((1.0 - t) * omega)/sinOmega
              endScale = math.sin(t * omega)/sinOmega
          else:
              # ends very close
              startScale = 1.0 - t
              endScale = t
          destQuat.setI(startScale * startQ.getI() +
                        endScale * endQuat.getI())
          destQuat.setJ(startScale * startQ.getJ() +
                        endScale * endQuat.getJ())
          destQuat.setK(startScale * startQ.getK() +
                        endScale * endQuat.getK())
          destQuat.setR(startScale * startQ.getR() +
                        endScale * endQuat.getR())
      else:
          # ends nearly opposite
          destQuat.setI(-startQ.getJ())
          destQuat.setJ(startQ.getI())
          destQuat.setK(-startQ.getR())
          destQuat.setR(startQ.getK())
          startScale = math.sin((0.5 - t) * math.pi)
          endScale = math.sin(t * math.pi)
          destQuat.setI(startScale * startQ.getI() +
                        endScale * endQuat.getI())
          destQuat.setJ(startScale * startQ.getJ() +
                        endScale * endQuat.getJ())
          destQuat.setK(startScale * startQ.getK() +
                        endScale * endQuat.getK())
      return destQuat
  
  