def pdmat3_to_npmat3(pdmat3: Mat3) -> npt.NDArray[float]:
"""
convert a mat3 matrix to a numpy 2darray...
:param pdmat3:
:return: numpy 2darray
author: weiwei
date: 20161216sapporo
"""
row0 = pdmat3.getRow(0)
row1 = pdmat3.getRow(1)
row2 = pdmat3.getRow(2)
return np.array([[row0[0], row1[0], row2[0]], [row0[1], row1[1], row2[1]],
[row0[2], row1[2], row2[2]]])
def color_from_temperature(temperature):
# Thanks to rdb for this conversion script
mm = 1000.0 / temperature # pylint: disable=invalid-name
mm2 = mm**2
mm3 = mm2 * mm
x, y = 0, 0
if temperature < 4000:
x = -0.2661239 * mm3 - 0.2343580 * mm2 + 0.8776956 * mm + 0.179910
else:
x = -3.0258469 * mm3 + 2.1070379 * mm2 + 0.2226347 * mm + 0.240390
x2 = x**2 # pylint: disable=invalid-name
x3 = x2 * x # pylint: disable=invalid-name
if temperature < 2222:
y = -1.1063814 * x3 - 1.34811020 * x2 + 2.18555832 * x - 0.20219683
elif temperature < 4000:
y = -0.9549476 * x3 - 1.37418593 * x2 + 2.09137015 * x - 0.16748867
else:
y = 3.0817580 * x3 - 5.87338670 * x2 + 3.75112997 * x - 0.37001483
# xyY to XYZ, assuming Y=1.
xyz = Vec3(x / y, 1, (1 - x - y) / y)
# Convert XYZ to linearized sRGB.
xyz_to_rgb = Mat3(3.2406, -0.9689, 0.0557, -1.5372, 1.8758, -0.2050,
-0.4986, 0.0415, 1.0570)
return xyz_to_rgb.xform(xyz)
def generateNormals(self):
'''
'''
self.varthickness = []
self.normals = []
last_normal = Vec3(0, 0, 1)
# last_vec = Vec3(0, 1, 0)
for i in range(len(self.track_points)):
if i == 0:
vec = self.track_points[0] - self.track_points[1]
elif i + 1 == len(self.track_points):
vec = self.track_points[i - 1] - self.track_points[0]
else:
vec = self.track_points[i - 1] - self.track_points[i + 1]
# calculate here the direction out of the street vector and the last normal
last_normal.normalize()
vec.normalize()
mat = Mat3()
mat.setRotateMat(-90, last_normal) # turn the direction around the last_normal
turned_vec = mat.xform(vec)
turned_vec.normalize()
last_normal = turned_vec.cross(vec) # calculate the new normal
turned_vec.normalize()
self.varthickness.append(turned_vec)
self.normals.append(last_normal)
def check_mouse1drag(self):
"""
this function uses a collision sphere to track the rotational mouse motion
:return:
author: weiwei
date: 20200315
"""
curm1pos = self.get_world_mouse1()
if curm1pos is None:
if self.lastm1pos is not None:
self.lastm1pos = None
return
if self.lastm1pos is None:
# first time click
self.lastm1pos = curm1pos
return
curm1vec = Vec3(curm1pos - self.lookatpos_pdv3)
lastm1vec = Vec3(self.lastm1pos - self.lookatpos_pdv3)
curm1vec.normalize()
lastm1vec.normalize()
rotatevec = curm1vec.cross(lastm1vec)
if rotatevec.length() > 1e-9: # avoid zero length
rotateangle = curm1vec.signedAngleDeg(lastm1vec, rotatevec)
rotateangle = rotateangle * self.camdist * 5000
if rotateangle > .02 or rotateangle < -.02:
rotmat = Mat4(self.base.cam.getMat())
posvec = Vec3(self.base.cam.getPos())
rotmat.setRow(3, Vec3(0, 0, 0))
self.base.cam.setMat(rotmat *
Mat4.rotateMat(rotateangle, rotatevec))
self.base.cam.setPos(Mat3.rotateMat(rotateangle, rotatevec). \
xform(posvec - self.lookatpos_pdv3) + self.lookatpos_pdv3)
self.lastm1pos = self.get_world_mouse1()
self.update_trackplane()
def step(self, task):
distance = ClockObject.getGlobalClock().getDt() * self.speed
rotMat = Mat3.rotateMatNormaxis(self.avatar.getH(), Vec3.up())
step = Vec3(rotMat.xform(Vec3.forward() * distance))
self.avatar.setFluidPos(Point3(self.avatar.getPos() + step))
return Task.cont
def pdquat_to_npmat3(pdquat):
"""
:param pdquat: panda.core.LQuaternion
:return:
author: weiwei
date: 20210109
"""
tmp_pdmat3 = Mat3()
pdquat.extractToMatrix(tmp_pdmat3)
return pdmat3_to_npmat3(tmp_pdmat3)
def npmat3_to_pdmat3(npmat3):
"""
convert numpy.2darray to LMatrix3f defined in Panda3d
:param npmat3: a 3x3 numpy ndarray
:return: a LMatrix3f object, see panda3d
author: weiwei
date: 20161107tsukuba
"""
return Mat3(npmat3[0, 0], npmat3[1, 0], npmat3[2, 0], \
npmat3[0, 1], npmat3[1, 1], npmat3[2, 1], \
npmat3[0, 2], npmat3[1, 2], npmat3[2, 2])
def _player_controls(self):
avel = self.body.getAngularVel()
on_plate = False
accelerate = False
for level_object in self.level.objects:
collide = OdeUtil.collide(level_object.geom, self.geom)
if collide:
on_plate = True
if avel.z < self.initial_spin_velocity and \
isinstance(level_object, Accelerator):
accelerate = level_object.player_interact(self)
break
if accelerate:
avel.z += self.exponent / 200.0
self.factor = 0
self.calculate_factor(avel.z)
if self.factor > 0.99:
return
f = self.factor**self.exponent
if on_plate:
if self.controls.jump:
vel = self.body.getLinearVel()
self.body.setLinearVel(vel.x, vel.y, vel.z + 5)
force = 350
else:
force = 25
self.controls.jump = False
self.body.addForce(Mat3.rotateMat(self.controls.view_rotation).xform(
(self.controls.y*force,
-0.8*self.controls.x*force,
0)))
hpr = self.model.getHpr()
self.model.setHpr(hpr.x, f*hpr.y, f*hpr.z)
self.body.setQuaternion(self.model.getQuat(render))
self.body.setAngularVel(f*avel.x,
f*avel.y,
avel.z)
def getCirclesBorder(cls, circle, numSlices=32, closed=False):
degPerSlice = 360.0 / numSlices
zVec = Vec3.unitZ()
rotMat = Mat3.rotateMat(degPerSlice, zVec)
projVec = Vec3.unitY() + circle.center
projVec.normalize()
border = [
Point3(projVec * circle.radius + circle.center),
]
projVec = rotMat.xform(projVec)
for i in range(1, numSlices):
pt = Point3(projVec * circle.radius + circle.center)
border.append(pt)
projVec = rotMat.xform(projVec)
if closed and border:
border.append(border[0])
return border
def _makeLineMat(self, x1, y1, x2, y2):
# This function generates a transformation matrix to convert coordinates
# from worldspace to be local to the provided line.
# This matrix will do the forward transformation. In other words, it
# transforms (0, 0) -> (x1, y1) and (0, 1) -> (x2, y2)
# N.B. the notation below is the transpose of the matrix I'm defining,
# because Panda3D's Mat3 constructor is column-major.
mat = Mat3(
y2-y1, x1-x2, 0,
x2-x1, y2-y1, 0,
x1, y1, 1)
# Now we invert it, so that it does what we want: the reverse
# transformation (i.e. (x1, y1) -> (0, 0) and (x2, y2) -> (0, 1))
if not mat.invertInPlace():
# The matrix is singular, which means it has no inverse.
return None
return mat
def test_transform_identity():
state = TransformState.make_identity()
assert state.is_identity()
assert not state.is_invalid()
assert not state.is_singular()
assert state.is_2d()
assert state.has_components()
assert not state.components_given()
assert not state.hpr_given()
assert not state.quat_given()
assert state.has_pos()
assert state.has_hpr()
assert state.has_quat()
assert state.has_scale()
assert state.has_identity_scale()
assert state.has_uniform_scale()
assert state.has_shear()
assert not state.has_nonzero_shear()
assert state.has_mat()
assert state.get_pos() == (0, 0, 0)
assert state.get_hpr() == (0, 0, 0)
assert state.get_quat() == (1, 0, 0, 0)
assert state.get_norm_quat() == (1, 0, 0, 0)
assert state.get_scale() == (1, 1, 1)
assert state.get_uniform_scale() == 1
assert state.get_shear() == (0, 0, 0)
assert state.get_mat() == Mat4.ident_mat()
assert state.get_pos2d() == (0, 0)
assert state.get_rotate2d() == 0
assert state.get_scale2d() == (1, 1)
assert state.get_shear2d() == 0
assert state.get_mat3() == Mat3.ident_mat()
state2 = TransformState.make_identity()
assert state.this == state2.this
def handleAvatarControls(self, task):
"""
Check on the arrow keys and update the avatar.
"""
# get the button states:
run = inputState.isSet("run")
forward = inputState.isSet("forward")
reverse = inputState.isSet("reverse")
turnLeft = inputState.isSet("turnLeft")
turnRight = inputState.isSet("turnRight")
slideLeft = inputState.isSet("slideLeft")
slideRight = inputState.isSet("slideRight")
jump = inputState.isSet("jump")
avatarControlForwardSpeed = 0
avatarControlReverseSpeed = 0
avatarControlSLeftSpeed = 0
avatarControlSRightSpeed = 0
if forward:
if not self.hasSetForwardInitTime:
self.forwardInitTime = globalClock.getFrameTime()
self.hasSetForwardInitTime = True
self.isAtZeroForward = False
self.hasSetForwardStopTime = False
timeSinceForwardSet = globalClock.getFrameTime() - self.forwardInitTime
if timeSinceForwardSet == 0:
avatarControlForwardSpeed = 0.0
else:
avatarControlForwardSpeed = min((timeSinceForwardSet / self.avatarControlForwardSpeed) * 750,
self.avatarControlForwardSpeed)
else:
if self.hasSetForwardInitTime:
self.hasSetForwardInitTime = False
if not self.hasSetForwardStopTime:
self.forwardStopTime = globalClock.getFrameTime()
self.hasSetForwardStopTime = True
timeSinceForwardStop = globalClock.getFrameTime() - self.forwardStopTime
if timeSinceForwardStop == 0:
avatarControlForwardSpeed = 16.0
elif not self.isAtZeroForward:
avatarControlForwardSpeed = self.avatarControlForwardSpeed - (timeSinceForwardStop * 50)
if avatarControlForwardSpeed <= 0.5:
avatarControlForwardSpeed = 0.0
self.isAtZeroForward = True
self.hasSetForwardStopTime = False
if reverse:
if not self.hasSetReverseInitTime:
self.reverseInitTime = globalClock.getFrameTime()
self.hasSetReverseInitTime = True
self.isAtZeroReverse = False
self.hasSetReverseStopTime = False
timeSinceReverseSet = globalClock.getFrameTime() - self.reverseInitTime
if timeSinceReverseSet == 0:
avatarControlReverseSpeed = 0.0
else:
avatarControlReverseSpeed = min((timeSinceReverseSet / self.avatarControlReverseSpeed) * 400,
self.avatarControlReverseSpeed)
else:
if self.hasSetReverseInitTime:
self.hasSetReverseInitTime = False
if not self.hasSetReverseStopTime:
self.reverseStopTime = globalClock.getFrameTime()
self.hasSetReverseStopTime = True
timeSinceReverseStop = globalClock.getFrameTime() - self.reverseStopTime
if timeSinceReverseStop == 0:
avatarControlReverseSpeed = 16.0
elif not self.isAtZeroReverse:
avatarControlReverseSpeed = self.avatarControlReverseSpeed - (timeSinceReverseStop * 20)
if avatarControlReverseSpeed <= 0.5:
avatarControlReverseSpeed = 0.0
self.isAtZeroReverse = True
self.hasSetReverseStopTime = False
if slideLeft:
if not self.hasSetSLeftInitTime:
self.sLeftInitTime = globalClock.getFrameTime()
self.hasSetSLeftInitTime = True
self.isAtZeroSLeft = False
self.hasSetSLeftStopTime = False
timeSinceSLeftSet = globalClock.getFrameTime() - self.sLeftInitTime
if timeSinceSLeftSet == 0:
avatarControlSLeftSpeed = 0.0
else:
avatarControlSLeftSpeed = min((timeSinceSLeftSet / self.avatarControlForwardSpeed) * 750,
self.avatarControlForwardSpeed)
else:
if self.hasSetSLeftInitTime:
self.hasSetSLeftInitTime = False
if not self.hasSetSLeftStopTime:
self.sLeftStopTime = globalClock.getFrameTime()
self.hasSetSLeftStopTime = True
timeSinceSLeftStop = globalClock.getFrameTime() - self.sLeftStopTime
if timeSinceSLeftStop == 0:
avatarControlSLeftSpeed = 16.0
elif not self.isAtZeroSLeft:
avatarControlSLeftSpeed = self.avatarControlForwardSpeed - (timeSinceSLeftStop * 50)
if avatarControlSLeftSpeed <= 0.5:
avatarControlSLeftSpeed = 0.0
self.isAtZeroSLeft = True
self.hasSetSLeftStopTime = False
if slideRight:
if not self.hasSetSRightInitTime:
self.sRightInitTime = globalClock.getFrameTime()
self.hasSetSRightInitTime = True
self.isAtZeroSRight = False
self.hasSetSRightStopTime = False
timeSinceSRightSet = globalClock.getFrameTime() - self.sRightInitTime
if timeSinceSRightSet == 0:
avatarControlSRightSpeed = 0.0
else:
avatarControlSRightSpeed = min((timeSinceSRightSet / self.avatarControlForwardSpeed) * 750,
self.avatarControlForwardSpeed)
else:
if self.hasSetSRightInitTime:
self.hasSetSRightInitTime = False
if not self.hasSetSRightStopTime:
self.sRightStopTime = globalClock.getFrameTime()
self.hasSetSRightStopTime = True
timeSinceSRightStop = globalClock.getFrameTime() - self.sRightStopTime
if timeSinceSRightStop == 0:
avatarControlSRightSpeed = 16.0
elif not self.isAtZeroSRight:
avatarControlSRightSpeed = self.avatarControlForwardSpeed - (timeSinceSRightStop * 50)
if avatarControlSRightSpeed <= 0.5:
avatarControlSRightSpeed = 0.0
self.isAtZeroSRight = True
self.hasSetSRightStopTime = False
# Check for Auto-Run
#if 'localAvatar' in __builtins__:
# if base.localAvatar and base.localAvatar.getAutoRun():
# forward = 1
# reverse = 0
# Determine what the speeds are based on the buttons:
self.speed=(avatarControlForwardSpeed or
-avatarControlReverseSpeed)
# Slide speed is a scaled down version of forward speed
# Note: you can multiply a factor in here if you want slide to
# be slower than normal walk/run. Let's try full speed.
#self.slideSpeed=(slideLeft and -self.avatarControlForwardSpeed*0.75 or
# slideRight and self.avatarControlForwardSpeed*0.75)
self.slideSpeed=(-avatarControlSLeftSpeed or
avatarControlSRightSpeed)
self.rotationSpeed=not (slideLeft or slideRight) and (
(turnLeft and self.avatarControlRotateSpeed) or
(turnRight and -self.avatarControlRotateSpeed))
if self.speed and self.slideSpeed:
self.speed *= GravityWalker.DiagonalFactor
self.slideSpeed *= GravityWalker.DiagonalFactor
debugRunning = inputState.isSet("debugRunning")
if(debugRunning):
self.speed*=base.debugRunningMultiplier
self.slideSpeed*=base.debugRunningMultiplier
self.rotationSpeed*=1.25
if self.needToDeltaPos:
self.setPriorParentVector()
self.needToDeltaPos = 0
if self.wantDebugIndicator:
self.displayDebugInfo()
if self.lifter.isOnGround():
if self.isAirborne:
self.isAirborne = 0
assert self.debugPrint("isAirborne 0 due to isOnGround() true")
impact = self.lifter.getImpactVelocity()
if impact < -30.0:
messenger.send("jumpHardLand")
self.startJumpDelay(0.3)
else:
messenger.send("jumpLand")
if impact < -5.0:
self.startJumpDelay(0.2)
# else, ignore the little potholes.
assert self.isAirborne == 0
self.priorParent = Vec3.zero()
if jump and self.mayJump:
# The jump button is down and we're close
# enough to the ground to jump.
self.lifter.addVelocity(self.avatarControlJumpForce)
messenger.send("jumpStart")
self.isAirborne = 1
assert self.debugPrint("isAirborne 1 due to jump")
else:
if self.isAirborne == 0:
assert self.debugPrint("isAirborne 1 due to isOnGround() false")
self.isAirborne = 1
self.__oldPosDelta = self.avatarNodePath.getPosDelta(render)
# How far did we move based on the amount of time elapsed?
self.__oldDt = ClockObject.getGlobalClock().getDt()
dt=self.__oldDt
# Check to see if we're moving at all:
self.moving = self.speed or self.slideSpeed or self.rotationSpeed or (self.priorParent!=Vec3.zero())
if self.moving:
distance = dt * self.speed
slideDistance = dt * self.slideSpeed
rotation = dt * self.rotationSpeed
# Take a step in the direction of our previous heading.
if distance or slideDistance or self.priorParent != Vec3.zero():
# rotMat is the rotation matrix corresponding to
# our previous heading.
rotMat=Mat3.rotateMatNormaxis(self.avatarNodePath.getH(), Vec3.up())
if self.isAirborne:
forward = Vec3.forward()
else:
contact = self.lifter.getContactNormal()
forward = contact.cross(Vec3.right())
# Consider commenting out this normalize. If you do so
# then going up and down slops is a touch slower and
# steeper terrain can cut the movement in half. Without
# the normalize the movement is slowed by the cosine of
# the slope (i.e. it is multiplied by the sign as a
# side effect of the cross product above).
forward.normalize()
self.vel=Vec3(forward * distance)
if slideDistance:
if self.isAirborne:
right = Vec3.right()
else:
right = forward.cross(contact)
# See note above for forward.normalize()
right.normalize()
self.vel=Vec3(self.vel + (right * slideDistance))
self.vel=Vec3(rotMat.xform(self.vel))
step=self.vel + (self.priorParent * dt)
self.avatarNodePath.setFluidPos(Point3(
self.avatarNodePath.getPos()+step))
self.avatarNodePath.setH(self.avatarNodePath.getH()+rotation)
else:
self.vel.set(0.0, 0.0, 0.0)
if self.moving or jump:
messenger.send("avatarMoving")
return Task.cont
def check_mouse1drag_trackball(self):
"""
This function uses the stereographic projection
introduced in https://en.wikipedia.org/wiki/Stereographic_projection to track the rotational mouse motion
Equations:
[e, f] -> [x, y, z] = [2e/(1+e^2+f^2), 2f/(1+e^2+f^2), (-1+e^2+f^2)/(2+2e^2+2f^2)]
:return:
author: weiwei
date: 20200315
"""
def cvtvirtualtrackball(screenx, screeny, psec_squared=1 / 4):
"""
convert a screen point to virtual trackball coordinate
psec indicates the size of the spherical section to be mapped to
default radius = 1
:param screenx:
:param screeny:
:param psec_squared:
:return:
author: weiwei
date: 20200315
"""
screenpoint_squaredsum = screenx**2 + screeny**2
trackballx = 2 * psec_squared * screenx / (psec_squared +
screenpoint_squaredsum)
trackballz = 2 * psec_squared * screeny / (psec_squared +
screenpoint_squaredsum)
trackbally = -math.sqrt(1 - trackballx**2 - trackballz**2)
returnvec = Vec3(trackballx, trackbally, trackballz)
returnvec.normalize()
return returnvec
currentmouse = self.base.mouseWatcherNode.getMouse()
curm1pos = [currentmouse.getX(), currentmouse.getY()]
if curm1pos is None:
if self.lastm1pos is not None:
self.lastm1pos = None
return
if self.lastm1pos is None:
# first time click
self.lastm1pos = curm1pos
return
curm1vec_pdv3 = cvtvirtualtrackball(curm1pos[0], curm1pos[1])
lastm1vec_pdv3 = cvtvirtualtrackball(self.lastm1pos[0],
self.lastm1pos[1])
rotatevec_pdv3 = curm1vec_pdv3.cross(lastm1vec_pdv3)
rotateangle = curm1vec_pdv3.signedAngleDeg(lastm1vec_pdv3,
rotatevec_pdv3)
if rotateangle > .02 or rotateangle < -.02:
rotateangle = rotateangle * 5
camrotmat_pd = self.base.cam.getMat().getUpper3()
calibrated_camrotmat_pd = Mat3.rotateMat(
rotateangle, camrotmat_pd.xformVec(rotatevec_pdv3))
posvec_pd = self.base.cam.getPos()
self.base.cam.setMat(Mat4.identMat())
self.base.cam.setMat(camrotmat_pd * calibrated_camrotmat_pd)
self.base.cam.setPos(
calibrated_camrotmat_pd.xform(posvec_pd -
self.lookatpos_pdv3) +
self.lookatpos_pdv3)
self.lastm1pos = curm1pos[:]
def _makeLineMat(self, x1, y1, x2, y2):
mat = Mat3(y2 - y1, x1 - x2, 0, x2 - x1, y2 - y1, 0, x1, y1, 1)
if not mat.invertInPlace():
return None
return mat