# Author: Shao Zhang, Phil Saltzman
# Last Updated: 5/2/2005
#
# This tutorial shows how to detect and respond to collisions. It uses solids
# create in code and the egg files, how to set up collision masks, a traverser,
# and a handler, how to detect collisions, and how to dispatch function based
# on the collisions. All of this is put together to simulate a labyrinth-style
# game

import direct.directbase.DirectStart
from pandac.PandaModules import *
from direct.interval.IntervalGlobal import *
from direct.gui.DirectGui import *
from direct.showbase.DirectObject import *
import sys

#Some constants for the program
ACCEL = 70         #Acceleration in ft/sec/sec
MAX_SPEED = 5      #Max speed in ft/sec
MAX_SPEED_SQ = MAX_SPEED ** 2  #Squared to make it easier to use lengthSquared
                               #Instead of length
UP = Vec3(0,0,1)   #We need this vector a lot, so its better to just have one
                   #instead of creating a new one every time we need it

class World(DirectObject):
  def __init__(self):
    #This code puts the standard title and instruction text on screen
    self.title = OnscreenText(text="Panda3D: Tutorial - Collision Detection",
                              style=1, fg=(1,1,1,1),
                              pos=(0.8,-0.95), scale = .07)
    self.instructions = OnscreenText(text="Mouse pointer tilts the board",
                                     pos = (-1.3, .95), fg=(1,1,1,1),
                                     align = TextNode.ALeft, scale = .05)
    
    self.accept("escape", sys.exit)        #Escape quits
    base.disableMouse()                    #Disable mouse-based camera control
    camera.setPosHpr(0, 0, 25, 0, -90, 0)  #Place the camera

    #Load the maze and place it in the scene
    self.maze = loader.loadModel("models/maze")
    self.maze.reparentTo(render)

    #Most times, you want collisions to be tested against invisible geometry
    #rather than every polygon. This is because testing against every polygon
    #in the scene is usually too slow. You can have simplified or approximate
    #geometry for the solids and still get good results.
    #
    #Sometimes you'll want to create and position your own collision solids in
    #code, but it's often easier to have them built automatically. This can be
    #done by adding special tags into an egg file. Check maze.egg and ball.egg
    #and look for lines starting with <Collide>. The part is brackets tells
    #Panda exactly what to do. Polyset means to use the polygons in that group
    #as solids, while Sphere tells panda to make a collision sphere around them
    #Keep means to keep the polygons in the group as visable geometry (good
    #for the ball, not for the triggers), and descend means to make sure that
    #the settings are applied to any subgroups.
    #
    #Once we have the collision tags in the models, we can get to them using
    #NodePath's find command

    #Find the collision node named wall_collide
    self.walls = self.maze.find("**/wall_collide")

    #Collision objects are sorted using BitMasks. BitMasks are ordinary numbers
    #with extra methods for working with them as binary bits. Every collision
    #solid has both a from mask and an into mask. Before Panda tests two
    #objects, it checks to make sure that the from and into collision masks
    #have at least one bit in common. That way things that shouldn't interact
    #won't. Normal model nodes have collision masks as well. By default they
    #are set to bit 20. If you want to collide against actual visable polygons,
    #set a from collide mask to include bit 20
    #
    #For this example, we will make everything we want the ball to collide with
    #include bit 0
    self.walls.node().setIntoCollideMask(BitMask32.bit(0))
    #CollisionNodes are usually invisible but can be shown. Uncomment the next
    #line to see the collision walls
    #self.walls.show()

    #We will now find the triggers for the holes and set their masks to 0 as
    #well. We also set their names to make them easier to identify during
    #collisions
    self.loseTriggers = []
    for i in range(6):
      trigger = self.maze.find("**/hole_collide" + str(i))
      trigger.node().setIntoCollideMask(BitMask32.bit(0))
      trigger.node().setName("loseTrigger")
      self.loseTriggers.append(trigger)
      #Uncomment this line to see the triggers
      #trigger.show()

    #Ground_collide is a single polygon on the same plane as the ground in the
    #maze. We will use a ray to collide with it so that we will know exactly
    #what height to put the ball at every frame. Since this is not something
    #that we want the ball itself to collide with, it has a different
    #bitmask.
    self.mazeGround = self.maze.find("**/ground_collide")
    self.mazeGround.node().setIntoCollideMask(BitMask32.bit(1))
    
    #Load the ball and attach it to the scene
    #It is on a root dummy node so that we can rotate the ball itself without
    #rotating the ray that will be attached to it
    self.ballRoot = render.attachNewNode("ballRoot")
    self.ball = loader.loadModel("models/ball")
    self.ball.reparentTo(self.ballRoot)

    #Find the collison sphere for the ball which was created in the egg file
    #Notice that it has a from collision mask of bit 0, and an into collison
    #mask of no bits. This means that the ball can only cause collisions, not
    #be collided into
    self.ballSphere = self.ball.find("**/ball")
    self.ballSphere.node().setFromCollideMask(BitMask32.bit(0))
    self.ballSphere.node().setIntoCollideMask(BitMask32.allOff())

    #No we create a ray to start above the ball and cast down. This is to
    #Determine the height the ball should be at and the angle the floor is
    #tilting. We could have used the sphere around the ball itself, but it
    #would not be as reliable
    self.ballGroundRay = CollisionRay()     #Create the ray
    self.ballGroundRay.setOrigin(0,0,10)    #Set its origin
    self.ballGroundRay.setDirection(0,0,-1) #And its direction
    #Collision solids go in CollisionNode
    self.ballGroundCol = CollisionNode('groundRay') #Create and name the node
    self.ballGroundCol.addSolid(self.ballGroundRay) #Add the ray
    self.ballGroundCol.setFromCollideMask(BitMask32.bit(1)) #Set its bitmasks
    self.ballGroundCol.setIntoCollideMask(BitMask32.allOff())
    #Attach the node to the ballRoot so that the ray is relative to the ball
    #(it will always be 10 feet over the ball and point down)
    self.ballGroundColNp = self.ballRoot.attachNewNode(self.ballGroundCol)
    #Uncomment this line to see the ray
    #self.ballGroundColNp.show()

    #Finally, we create a CollisionTraverser. CollisionTraversers are what
    #do the job of calculating collisions
    self.cTrav = CollisionTraverser()
    #Collision traverservs tell collision handlers about collisions, and then
    #the handler decides what to do with the information. We are using a
    #CollisionHandlerQueue, which simply creates a list of all of the
    #collisions in a given pass. There are more sophisticated handlers like
    #one that sends events and another that tries to keep collided objects
    #apart, but the results are often better with a simple queue
    self.cHandler = CollisionHandlerQueue()
    #Now we add the collision nodes that can create a collision to the
    #traverser. The traverser will compare these to all others nodes in the
    #scene. There is a limit of 32 CollisionNodes per traverser
    #We add the collider, and the handler to use as a pair
    self.cTrav.addCollider(self.ballSphere, self.cHandler)
    self.cTrav.addCollider(self.ballGroundColNp, self.cHandler)

    #Collision traversers have a built in tool to help visualize collisions.
    #Uncomment the next line to see it.
    #self.cTrav.showCollisions(render)
    
    #This section deals with lighting for the ball. Only the ball was lit
    #because the maze has static lighting pregenerated by the modeler
    lAttrib = LightAttrib.makeAllOff()
    ambientLight = AmbientLight( "ambientLight" )
    ambientLight.setColor( Vec4(.55, .55, .55, 1) )
    lAttrib = lAttrib.addLight( ambientLight )
    directionalLight = DirectionalLight( "directionalLight" )
    directionalLight.setDirection( Vec3( 0, 0, -1 ) )
    directionalLight.setColor( Vec4( 0.375, 0.375, 0.375, 1 ) )
    directionalLight.setSpecularColor(Vec4(1,1,1,1))
    lAttrib = lAttrib.addLight( directionalLight )
    self.ballRoot.node().setAttrib( lAttrib )
    
    #This section deals with adding a specular highlight to the ball to make
    #it look shiny
    m = Material()
    m.setSpecular(Vec4(1,1,1,1))
    m.setShininess(96)
    self.ball.setMaterial(m, 1)

    #Finally, we call start for more initialization
    self.start()

  def start(self):
    #The maze model also has a locator in it for where to start the ball
    #To access it we use the find command
    startPos = self.maze.find("**/start").getPos()
    self.ballRoot.setPos(startPos)   #Set the ball in the starting position
    self.ballV = Vec3(0,0,0)         #Initial velocity is 0
    self.accelV = Vec3(0,0,0)        #Initial acceleration is 0
    
    #For a traverser to actually do collisions, you need to call
    #traverser.traverse() on a part of the scene. Fortunatly, base has a
    #task that does this for the entire scene once a frame. This sets up our
    #traverser as the one to be called automatically
    base.cTrav = self.cTrav

    #Create the movement task, but first make sure it is not already running
    taskMgr.remove("rollTask")
    self.mainLoop = taskMgr.add(self.rollTask, "rollTask")
    self.mainLoop.last = 0

  #This function handles the collision between the ray and the ground
  #Information about the interaction is passed in colEntry
  def groundCollideHandler(self, colEntry):
    #Set the ball to the appropriate Z value for it to be exactly on the ground
    newZ = colEntry.getSurfacePoint(render).getZ()
    self.ballRoot.setZ(newZ+.4)

    #Find the acceleration direction. First the surface normal is crossed with
    #the up vector to get a vector perpendicular to the slope
    norm = colEntry.getSurfaceNormal(render)
    accelSide = norm.cross(UP)
    #Then that vector is crossed with the surface normal to get a vector that
    #points down the slope. By getting the acceleration in 3D like this rather
    #than in 2D, we reduce the amount of error per-frame, reducing jitter
    self.accelV = norm.cross(accelSide)

  #This function handles the collision between the ball and a wall
  def wallCollideHandler(self, colEntry):
    #First we calculate some numbers we need to do a reflection
    norm = colEntry.getSurfaceNormal(render) * -1 #The normal of the wall
    curSpeed = self.ballV.length()                #The current speed
    inVec = self.ballV / curSpeed                 #The direction of travel
    velAngle = norm.dot(inVec)                    #Angle of incidance
    hitDir = colEntry.getSurfacePoint(render) - self.ballRoot.getPos()
    hitDir.normalize()                            
    hitAngle = norm.dot(hitDir)   #The angle between the ball and the normal

    #Ignore the collision if the ball is either moving away from the wall
    #already (so that we don't accidentally send it back into the wall)
    #and ignore it if the collision isn't dead-on (to avoid getting caught on
    #corners)
    if velAngle > 0 and hitAngle > .995:
      #Standard reflection equation
      reflectVec = (norm * norm.dot(inVec * -1) * 2) + inVec
        
      #This makes the velocity half of what it was if the hit was dead-on
      #and nearly exactly what it was if this is a glancing blow
      self.ballV = reflectVec * (curSpeed * (((1-velAngle)*.5)+.5))
      #Since we have a collision, the ball is already a little bit buried in
      #the wall. This calculates a vector needed to move it so that it is
      #exactly touching the wall
      disp = (colEntry.getSurfacePoint(render) -
              colEntry.getInteriorPoint(render))
      newPos = self.ballRoot.getPos() + disp
      self.ballRoot.setPos(newPos)

  #This is the task that deals with making everything interactive
  def rollTask(self, task):
    #Standard technique for finding the amount of time since the last frame
    dt = task.time - task.last
    task.last = task.time

    #If dt is large, then there has been a #hiccup that could cause the ball
    #to leave the field if this functions runs, so ignore the frame
    if dt > .2: return Task.cont   

    #The collision handler collects the collisions. We dispatch which function
    #to handle the collision based on the name of what was collided into
    for i in range(self.cHandler.getNumEntries()):
      entry = self.cHandler.getEntry(i)
      name = entry.getIntoNode().getName()
      if   name == "wall_collide":   self.wallCollideHandler(entry)
      elif name == "ground_collide": self.groundCollideHandler(entry)
      elif name == "loseTrigger":    self.loseGame(entry)

    #Read the mouse position and tilt the maze accordingly
    if base.mouseWatcherNode.hasMouse():
      mpos = base.mouseWatcherNode.getMouse() #get the mouse position
      self.maze.setP(mpos.getY() * -10)
      self.maze.setR(mpos.getX() * 10)

    #Finally, we move the ball
    #Update the velocity based on acceleration
    self.ballV += self.accelV * dt * ACCEL
    #Clamp the velocity to the maximum speed
    if self.ballV.lengthSquared() > MAX_SPEED_SQ:
      self.ballV.normalize()
      self.ballV *= MAX_SPEED
    #Update the position based on the velocity
    self.ballRoot.setPos(self.ballRoot.getPos() + (self.ballV * dt))

    #This block of code rotates the ball. It uses something called a quaternion
    #to rotate the ball around an arbitrary axis. That axis perpendicular to
    #the balls rotation, and the amount has to do with the size of the ball
    #This is multiplied on the previous rotation to incrimentally turn it.
    prevRot = LRotationf(self.ball.getQuat())
    axis = UP.cross(self.ballV)
    newRot = LRotationf(axis, 45.5 * dt * self.ballV.length())
    self.ball.setQuat(prevRot * newRot)
    
    return Task.cont       #Continue the task indefinitely

  #If the ball hits a hole trigger, then it should fall in the hole.
  #This is faked rather than dealing with the actual physics of it.
  def loseGame(self, entry):
    #The triggers are set up so that the center of the ball should move to the
    #collision point to be in the hole
    toPos = entry.getInteriorPoint(render)
    taskMgr.remove('rollTask')  #Stop the maze task

    #Move the ball into the hole over a short sequence of time. Then wait a
    #second and call start to reset the game
    Sequence(
      Parallel(
      LerpFunc(self.ballRoot.setX, fromData = self.ballRoot.getX(),
               toData = toPos.getX(), duration = .1),
      LerpFunc(self.ballRoot.setY, fromData = self.ballRoot.getY(),
               toData = toPos.getY(), duration = .1),
      LerpFunc(self.ballRoot.setZ, fromData = self.ballRoot.getZ(),
               toData = self.ballRoot.getZ() - .9, duration = .2)),
      Wait(1),
      Func(self.start)).start()

#Finally, create an instance of our class and start 3d rendering
w = World()
run()
