def Torus(mNumSegSection=64, mNumSegCircle=64, mRadius=1.0,
mSectionRadius=0.2):
(gvw, prim, geom, path) = empty('t')
deltaSection = (pi * 2 / mNumSegSection)
deltaCircle = (pi * 2 / mNumSegCircle)
offset = 0
for i in range(0, mNumSegCircle + 1):
for j in range(0, mNumSegSection + 1):
v0 = Vec3(mRadius + mSectionRadius * cos(j * deltaSection),
mSectionRadius * sin(j * deltaSection), 0.0)
q = Quat()
q.setFromAxisAngleRad(i * deltaCircle, Vec3(0, 1, 0))
v = q.xform(v0)
gvw.addData3f(v)
if i != mNumSegCircle:
prim.addVertices(offset + mNumSegSection + 1, offset,
offset + mNumSegSection)
prim.addVertices(offset + mNumSegSection + 1, offset + 1,
offset)
offset += 1
prim.closePrimitive()
geom.addPrimitive(prim)
return path
def setTextureRotation(self, angle):
# Rotate texture around the face normal
rads = deg2Rad(self.rotation - angle)
# Rotate around the face normal
texNorm = self.vAxis.cross(self.uAxis).normalized()
transform = Quat()
transform.setFromAxisAngleRad(rads, texNorm)
self.uAxis = transform.xform(self.uAxis)
self.vAxis = transform.xform(self.vAxis)
self.rotation = angle
def loadModels(self):
self.limbModel = loader.loadModel("models/limb/limb.bam")
hw = 10.0
hh = 20.0
height = 27.0
legStarts = [
Vec3(hw, hh, height),
Vec3(-hw, hh, height),
Vec3(hw, -hh, height),
Vec3(-hw, -hh, height)
]
self.limbs = []
self.angles = 12 * [0.0]
self.rest_rots = []
for i in range(4):
femur = copy(self.limbModel)
femur.setPos(legStarts[i])
femur.reparentTo(render)
self.limbs.append(femur)
q = Quat()
q.setFromAxisAngleRad(np.pi * 0.75, Vec3(1.0, 0.0, 0.0))
self.rest_rots.append(q)
tibia = copy(self.limbModel)
tibia.setPos(Vec3(0.0, 0.0, 24.0))
tibia.reparentTo(femur)
self.limbs.append(tibia)
q = Quat()
q.setFromAxisAngleRad(np.pi * 0.5, Vec3(1.0, 0.0, 0.0))
self.rest_rots.append(q)
self.floorModel = loader.loadModel("models/floor/floor.bam")
self.floorModel.reparentTo(render)
def mainLoop(self, task):
camera.setPos(Vec3(-150.0, 150.0, 100.0))
camera.lookAt(Vec3(0.0, 0.0, 10.0))
ANGLE_RES = self.h.getInputSize(0).z
self.h.step(self.cs, [self.h.getPredictionCs(0)], False, 1.0)
for i in range(12):
target_angle = (self.h.getPredictionCs(0)[i] /
float(ANGLE_RES - 1) * 2.0 - 1.0) * (0.3 * np.pi)
self.angles[i] += 0.1 * (target_angle - self.angles[i])
# Adjust angles
for i in range(4):
q1 = Quat()
q1.setFromAxisAngleRad(self.angles[0 * 4 + i], Vec3(0.0, 1.0, 0.0))
q2 = Quat()
q2.setFromAxisAngleRad(-self.angles[1 * 4 + i],
Vec3(1.0, 0.0, 0.0))
self.limbs[i * 2 + 0].setQuat(self.rest_rots[i * 2 + 0] * q1 * q2)
q1 = Quat()
q1.setFromAxisAngleRad(-self.angles[2 * 4 + i],
Vec3(1.0, 0.0, 0.0))
self.limbs[i * 2 + 1].setQuat(self.rest_rots[i * 2 + 1] * q1)
return task.cont
def getRotationTo(src, dest, fallbackAxis=Vec3_ZERO):
# Quaternion q;
# Vector3 v0 = *this;
# Vector3 v1 = dest;
# v0.normalise();
# v1.normalise();
q = Quat()
v0 = Vec3(src)
v1 = Vec3(dest)
v0.normalize()
v1.normalize()
# Real d = v0.dotProduct(v1);
d = v0.dot(v1)
# if (d >= 1.0f)
# {
# return Quaternion::IDENTITY;
# }
if d >= 1.0:
return Quat(1, 0, 0, 0)
# if (d < (1e-6f - 1.0f))
if d < (1.0e-6 - 1.0):
# if (fallbackAxis != Vector3::ZERO)
# {
# // rotate 180 degrees about the fallback axis
# q.FromAngleAxis(Radian(Math::PI), fallbackAxis);
# }
if fallbackAxis != Vec3_ZERO:
q.setFromAxisAngleRad(pi, fallbackAxis)
# else
# {
# // Generate an axis
# Vector3 axis = Vector3::UNIT_X.crossProduct(*this);
# if (axis.isZeroLength()) // pick another if colinear
# axis = Vector3::UNIT_Y.crossProduct(*this);
# axis.normalise();
# q.FromAngleAxis(Radian(Math::PI), axis);
# }
else:
axis = Vec3(1, 0, 0).cross(src)
if axis.almostEqual(Vec3.zero()):
axis = Vec3(0, 1, 0).cross(src)
axis.normalize()
q.setFromAxisAngleRad(pi, axis)
# else
# {
# Real s = Math::Sqrt( (1+d)*2 );
# Real invs = 1 / s;
# Vector3 c = v0.crossProduct(v1);
# q.x = c.x * invs;
# q.y = c.y * invs;
# q.z = c.z * invs;
# q.w = s * 0.5f;
# q.normalise();
# }
else:
s = sqrt((1 + d) * 2)
invs = 1 / s
c = v0.cross(v1)
q.setI(c.x * invs)
q.setJ(c.y * invs)
q.setK(c.z * invs)
q.setR(s * .5)
q.normalize()
return q
