def _createCylinder(proxy, axis, basePos, tipPos, radius, colour, moiScale, withMesh): """ Private function. Use createCylinder() or createArticulatedCylinder() instead. """ if axis != 0 and axis != 1 and axis != 2: raise ValueError('Axis must be 0 for x, 1 for y or 2 for z.') # Mesh and cdps will be set manually proxy.meshes = None proxy.cdps = None # Compute box moi moi = [0, 0, 0] height = math.fabs(tipPos - basePos) for i in range(3): if i == axis: moi[i] = proxy.mass * radius * radius / 2.0 else: moi[i] = proxy.mass * (3 * radius * radius + height * height) / 12.0 ### HACK! moi[i] = max(moi[i], 0.01) proxy.moi = PyUtils.toVector3d(moi) * moiScale cylinder = proxy.createAndFillObject() basePoint = [0, 0, 0] tipPoint = [0, 0, 0] basePoint[axis] = basePos tipPoint[axis] = tipPos basePoint3d = PyUtils.toPoint3d(basePoint) tipPoint3d = PyUtils.toPoint3d(tipPoint) baseToTipVector3d = Vector3d(basePoint3d, tipPoint3d) if baseToTipVector3d.isZeroVector(): raise ValueError( 'Invalid points for cylinder: base and tip are equal!') baseToTipUnitVector3d = baseToTipVector3d.unit() if height <= radius * 2.0: cdp = Physics.SphereCDP() cdp.setCenter(basePoint3d + baseToTipVector3d * 0.5) cdp.setRadius(height / 2.0) else: cdp = Physics.CapsuleCDP() cdp.setPoint1(basePoint3d + baseToTipUnitVector3d * radius) cdp.setPoint2(tipPoint3d + baseToTipUnitVector3d * -radius) cdp.setRadius(radius) cylinder.addCollisionDetectionPrimitive(cdp) if withMesh: mesh = Mesh.createCylinder(basePoint, tipPoint, radius, colour) cylinder.addMesh(mesh) return cylinder
def _createCylinder(proxy, axis, basePos, tipPos, radius, colour, moiScale, withMesh): """ Private function. Use createCylinder() or createArticulatedCylinder() instead. """ if axis != 0 and axis != 1 and axis != 2: raise ValueError("Axis must be 0 for x, 1 for y or 2 for z.") # Mesh and cdps will be set manually proxy.meshes = None proxy.cdps = None # Compute box moi moi = [0, 0, 0] height = math.fabs(tipPos - basePos) for i in range(3): if i == axis: moi[i] = proxy.mass * radius * radius / 2.0 else: moi[i] = proxy.mass * (3 * radius * radius + height * height) / 12.0 ### HACK! moi[i] = max(moi[i], 0.01) proxy.moi = PyUtils.toVector3d(moi) * moiScale cylinder = proxy.createAndFillObject() basePoint = [0, 0, 0] tipPoint = [0, 0, 0] basePoint[axis] = basePos tipPoint[axis] = tipPos basePoint3d = PyUtils.toPoint3d(basePoint) tipPoint3d = PyUtils.toPoint3d(tipPoint) baseToTipVector3d = Vector3d(basePoint3d, tipPoint3d) if baseToTipVector3d.isZeroVector(): raise ValueError("Invalid points for cylinder: base and tip are equal!") baseToTipUnitVector3d = baseToTipVector3d.unit() if height <= radius * 2.0: cdp = Physics.SphereCDP() cdp.setCenter(basePoint3d + baseToTipVector3d * 0.5) cdp.setRadius(height / 2.0) else: cdp = Physics.CapsuleCDP() cdp.setPoint1(basePoint3d + baseToTipUnitVector3d * radius) cdp.setPoint2(tipPoint3d + baseToTipUnitVector3d * -radius) cdp.setRadius(radius) cylinder.addCollisionDetectionPrimitive(cdp) if withMesh: mesh = Mesh.createCylinder(basePoint, tipPoint, radius, colour) cylinder.addMesh(mesh) return cylinder
def createCylinder(basePoint=(0, -1, 0), tipPoint=(0, 1, 0), radius=1.0, colour=(0.6, 0.6, 0.6), samples=20): """ Creates the mesh for a cylinder between the two specified points. Colour should be a 3-tuple (R,G,B) or a 4-tuple (R,G,B,A) """ basePoint = PyUtils.toPoint3d(basePoint) tipPoint = PyUtils.toPoint3d(tipPoint) baseToTipVector = Vector3d(basePoint, tipPoint) if baseToTipVector.isZeroVector(): raise ValueError( 'Invalid points for cylinder: base and tip are equal!') baseToTipUnitVector = baseToTipVector.unit() xUnitVector = baseToTipUnitVector.crossProductWith(Vector3d(0, 0, 1)) if xUnitVector.length() < 0.5: xUnitVector = baseToTipUnitVector.crossProductWith(Vector3d(0, -1, 0)) xUnitVector.toUnit() yUnitVector = baseToTipUnitVector.crossProductWith(Vector3d(-1, 0, 0)) if yUnitVector.length() < 0.5: yUnitVector = baseToTipUnitVector.crossProductWith(Vector3d(0, 1, 0)) yUnitVector.toUnit() vertices = [] for i in range(samples): theta = i * 2 * math.pi / float(samples) vertices.append(basePoint + xUnitVector * math.cos(theta) * radius + yUnitVector * math.sin(theta) * radius) for i in range(samples): theta = i * 2 * math.pi / float(samples) vertices.append(tipPoint + xUnitVector * math.cos(theta) * radius + yUnitVector * math.sin(theta) * radius) for i in range(samples): theta = i * 2 * math.pi / float(samples) vertices.append(basePoint + xUnitVector * math.cos(theta) * radius + yUnitVector * math.sin(theta) * radius) vertices.append(tipPoint + xUnitVector * math.cos(theta) * radius + yUnitVector * math.sin(theta) * radius) faces = [range(0, samples), range(samples, 2 * samples)] for i in range(0, 2 * samples, 2): base = 2 * samples size = 2 * samples faces.append((base + i, base + i + 1, base + (i + 3) % size, base + (i + 2) % size)) return create(vertices, faces, colour)
def createBox(size=(1, 1, 1), position=(0, 0, 0), colour=(0.6, 0.6, 0.6)): """ Creates the mesh for a box having the specified size and a specified position. The size should be a 3-tuple (xSize, ySize, zSize). The position should be a 3-tuple. Colour should be a 3-tuple (R,G,B) or a 4-tuple (R,G,B,A) """ size = PyUtils.toVector3d(size) position = PyUtils.toPoint3d(position) vertices = [] delta = MathLib.Vector3d() for repeat in range(3): for x in (-0.5, 0.5): delta.x = size.x * x for y in (-0.5, 0.5): delta.y = size.y * y for z in (-0.5, 0.5): delta.z = size.z * z vertices.append(position + delta) faces = [ (0, 1, 3, 2), (5, 4, 6, 7), # YZ Faces (9, 13, 15, 11), (12, 8, 10, 14), # XY Faces (18, 19, 23, 22), (17, 16, 20, 21) ] # XZ Faces return create(vertices, faces, colour)
def create(vertices, faces, colour=(0.6, 0.6, 0.6)): """ Creates a mesh having the specified vertices and faces. Vertices should be a list of 3-tuples of float or Point3d (positions). Faces should be a list of tuples of indices. Colour should be a 3-tuple (R,G,B) or a 4-tuple (R,G,B,A) """ mesh = GLUtils.GLMesh() for vertex in vertices: mesh.addVertex(PyUtils.toPoint3d(vertex)) for face in faces: poly = GLUtils.GLIndexedPoly() for index in face: poly.addVertexIndex(index) mesh.addPoly(poly) try: mesh.setColour(*colour) except TypeError: mesh.setColour(*(colour + (1, ))) mesh.computeNormals() return mesh
def load(self): assert not self._loaded, "Cannot load scenario twice!" self._loaded = True # Create the rigid bodies for the main staircase orientation = PyUtils.angleAxisToQuaternion( (self._angle,(0,1,0)) ) size = MathLib.Vector3d( self._staircaseWidth, self._riserHeight, self._threadDepth ) pos = PyUtils.toPoint3d( self._position ) + MathLib.Vector3d( 0, -self._riserHeight/2.0, 0 ) delta = MathLib.Vector3d(size) delta.x = 0 delta = orientation.rotate( delta ) for i in range(self._stepCount): box = PyUtils.RigidBody.createBox( size, pos = pos + delta * (i+1), locked = True, orientation=orientation ) Physics.world().addRigidBody(box) # Create the rigid bodies for both ramps rampHeights = ( self._leftRampHeight, self._rightRampHeight ) deltaRamp = MathLib.Vector3d(self._staircaseWidth/2.0,0,0) deltaRamp = orientation.rotate( deltaRamp ) deltaRamps = (deltaRamp, deltaRamp * -1) for deltaRamp, rampHeight in zip( deltaRamps, rampHeights ): if rampHeight is None: continue deltaRamp.y = rampHeight/2.0 box = PyUtils.RigidBody.createBox( (0.02,rampHeight,0.02), pos = pos + deltaRamp + delta , locked = True, orientation=orientation ) Physics.world().addRigidBody(box) box = PyUtils.RigidBody.createBox( (0.02,rampHeight,0.02), pos = pos + deltaRamp + (delta * self._stepCount) , locked = True, orientation=orientation ) Physics.world().addRigidBody(box) deltaRamp.y = rampHeight rampOrientation = orientation * PyUtils.angleAxisToQuaternion( (math.atan2(self._riserHeight, self._threadDepth), (-1,0,0)) ) rampLen = self._stepCount * math.sqrt( self._riserHeight*self._riserHeight + self._threadDepth*self._threadDepth ) box = PyUtils.RigidBody.createBox( (0.04,0.02,rampLen), pos = pos + deltaRamp + (delta * ((self._stepCount+1) * 0.5)) , locked = True, orientation=rampOrientation ) Physics.world().addRigidBody(box)
def createBox( size=(1,1,1), position=(0,0,0), colour=(0.6,0.6,0.6) ): """ Creates the mesh for a box having the specified size and a specified position. The size should be a 3-tuple (xSize, ySize, zSize). The position should be a 3-tuple. Colour should be a 3-tuple (R,G,B) or a 4-tuple (R,G,B,A) """ size = PyUtils.toVector3d(size) position = PyUtils.toPoint3d(position) vertices = [] delta = MathLib.Vector3d() for repeat in range(3): for x in (-0.5,0.5) : delta.x = size.x * x for y in (-0.5,0.5) : delta.y = size.y * y for z in (-0.5,0.5) : delta.z = size.z * z vertices.append( position + delta ) faces = [(0,1,3,2),(5,4,6,7), # YZ Faces (9,13,15,11),(12,8,10,14), # XY Faces (18,19,23,22),(17,16,20,21)] # XZ Faces return create( vertices, faces, colour )
def create( vertices, faces, colour=(0.6,0.6,0.6) ): """ Creates a mesh having the specified vertices and faces. Vertices should be a list of 3-tuples of float or Point3d (positions). Faces should be a list of tuples of indices. Colour should be a 3-tuple (R,G,B) or a 4-tuple (R,G,B,A) """ mesh = GLUtils.GLMesh() for vertex in vertices: mesh.addVertex( PyUtils.toPoint3d(vertex) ) for face in faces: poly = GLUtils.GLIndexedPoly() for index in face: poly.addVertexIndex( index ) mesh.addPoly(poly) try: mesh.setColour( *colour ) except TypeError: mesh.setColour( *(colour + (1,)) ) mesh.computeNormals() return mesh
def createEllipsoid(position=(0, 0, 0), radius=(1, 1, 1), colour=(0.6, 0.6, 0.6), samplesY=20, samplesXZ=20, exponentBottom=2, exponentTop=2, exponentSide=2): """ Creates the mesh for an ellipsoid having the specified position and radius Colour should be a 3-tuple (R,G,B) or a 4-tuple (R,G,B,A) """ if exponentBottom < 2.0 or exponentTop < 2.0 or exponentSide < 2.0: raise ValueError('Exponents for ellipsoid must all be under 2.0!') position = PyUtils.toPoint3d(position) vertices = [] for i in range(1, samplesY): thetaI = i * math.pi / float(samplesY) if i < samplesY / 2: n = exponentTop else: n = exponentBottom cos = math.cos(thetaI) y = cos * radius[1] scaleXZ = math.pow(1 - math.pow(math.fabs(cos), n), 1.0 / float(n)) for j in range(0, samplesXZ): thetaJ = j * 2.0 * math.pi / float(samplesXZ) n = exponentSide cos = math.cos(thetaJ) x = cos * scaleXZ * radius[0] z = math.pow(1 - math.pow(math.fabs(cos), n), 1.0 / float(n)) * math.copysign( 1, math.sin(thetaJ)) * scaleXZ * radius[2] vertices.append(position + Vector3d(x, y, z)) vertices.append(position + Vector3d(0, radius[1], 0)) vertices.append(position + Vector3d(0, -radius[1], 0)) faces = [] for i in range(0, (samplesY - 2) * samplesXZ, samplesXZ): for j in range(0, samplesXZ): faces.append( (i + j, i + (j + 1) % samplesXZ, i + samplesXZ + (j + 1) % samplesXZ, i + samplesXZ + j)) for i in range(0, samplesXZ): base = (samplesY - 2) * samplesXZ faces.append(((i + 1) % samplesXZ, i, (samplesY - 1) * samplesXZ)) faces.append((base + i, base + (i + 1) % samplesXZ, (samplesY - 1) * samplesXZ + 1)) return create(vertices, faces, colour)
def load(self): assert not self._loaded, "Cannot load scenario twice!" self._loaded = True # Create the rigid bodies for the main staircase orientation = PyUtils.angleAxisToQuaternion((self._angle, (0, 1, 0))) size = MathLib.Vector3d(self._staircaseWidth, self._riserHeight, self._threadDepth) pos = PyUtils.toPoint3d(self._position) + MathLib.Vector3d( 0, -self._riserHeight / 2.0, 0) delta = MathLib.Vector3d(size) delta.x = 0 delta = orientation.rotate(delta) for i in range(self._stepCount): box = PyUtils.RigidBody.createBox(size, pos=pos + delta * (i + 1), locked=True, orientation=orientation) Physics.world().addRigidBody(box) # Create the rigid bodies for both ramps rampHeights = (self._leftRampHeight, self._rightRampHeight) deltaRamp = MathLib.Vector3d(self._staircaseWidth / 2.0, 0, 0) deltaRamp = orientation.rotate(deltaRamp) deltaRamps = (deltaRamp, deltaRamp * -1) for deltaRamp, rampHeight in zip(deltaRamps, rampHeights): if rampHeight is None: continue deltaRamp.y = rampHeight / 2.0 box = PyUtils.RigidBody.createBox((0.02, rampHeight, 0.02), pos=pos + deltaRamp + delta, locked=True, orientation=orientation) Physics.world().addRigidBody(box) box = PyUtils.RigidBody.createBox( (0.02, rampHeight, 0.02), pos=pos + deltaRamp + (delta * self._stepCount), locked=True, orientation=orientation) Physics.world().addRigidBody(box) deltaRamp.y = rampHeight rampOrientation = orientation * PyUtils.angleAxisToQuaternion( (math.atan2(self._riserHeight, self._threadDepth), (-1, 0, 0))) rampLen = self._stepCount * math.sqrt( self._riserHeight * self._riserHeight + self._threadDepth * self._threadDepth) box = PyUtils.RigidBody.createBox( (0.04, 0.02, rampLen), pos=pos + deltaRamp + (delta * ((self._stepCount + 1) * 0.5)), locked=True, orientation=rampOrientation) Physics.world().addRigidBody(box)
def createCylinder( basePoint=(0,-1,0), tipPoint=(0,1,0), radius = 1.0, colour=(0.6,0.6,0.6), samples = 20 ): """ Creates the mesh for a cylinder between the two specified points. Colour should be a 3-tuple (R,G,B) or a 4-tuple (R,G,B,A) """ basePoint = PyUtils.toPoint3d(basePoint) tipPoint = PyUtils.toPoint3d(tipPoint) baseToTipVector = Vector3d(basePoint,tipPoint) if baseToTipVector.isZeroVector() : raise ValueError( 'Invalid points for cylinder: base and tip are equal!' ) baseToTipUnitVector = baseToTipVector.unit() xUnitVector = baseToTipUnitVector.crossProductWith( Vector3d(0,0,1) ) if xUnitVector.length() < 0.5 : xUnitVector = baseToTipUnitVector.crossProductWith( Vector3d(0,-1,0) ) xUnitVector.toUnit() yUnitVector = baseToTipUnitVector.crossProductWith( Vector3d(-1,0,0) ) if yUnitVector.length() < 0.5 : yUnitVector = baseToTipUnitVector.crossProductWith( Vector3d(0,1,0) ) yUnitVector.toUnit() vertices = [] for i in range(samples): theta = i * 2 * math.pi / float(samples) vertices.append( basePoint + xUnitVector * math.cos(theta) * radius + yUnitVector * math.sin(theta) * radius ) for i in range(samples): theta = i * 2 * math.pi / float(samples) vertices.append( tipPoint + xUnitVector * math.cos(theta) * radius + yUnitVector * math.sin(theta) * radius ) for i in range(samples): theta = i * 2 * math.pi / float(samples) vertices.append( basePoint + xUnitVector * math.cos(theta) * radius + yUnitVector * math.sin(theta) * radius ) vertices.append( tipPoint + xUnitVector * math.cos(theta) * radius + yUnitVector * math.sin(theta) * radius ) faces = [ range(0,samples), range(samples,2*samples) ] for i in range(0,2*samples,2) : base = 2*samples size = 2*samples faces.append( (base+i, base+i+1, base+(i+3)%size, base+(i+2)%size ) ) return create( vertices, faces, colour )
def createEllipsoid( position=(0,0,0), radius=(1,1,1), colour=(0.6,0.6,0.6), samplesY = 20, samplesXZ = 20, exponentBottom = 2, exponentTop = 2, exponentSide = 2 ): """ Creates the mesh for an ellipsoid having the specified position and radius Colour should be a 3-tuple (R,G,B) or a 4-tuple (R,G,B,A) """ if exponentBottom < 2.0 or exponentTop < 2.0 or exponentSide < 2.0 : raise ValueError( 'Exponents for ellipsoid must all be under 2.0!' ) position = PyUtils.toPoint3d(position) vertices = [] for i in range(1,samplesY): thetaI = i*math.pi/float(samplesY) if i < samplesY / 2 : n = exponentTop else: n = exponentBottom cos = math.cos(thetaI) y = cos * radius[1] scaleXZ = math.pow( 1-math.pow(math.fabs(cos),n), 1.0/float(n) ) for j in range(0,samplesXZ): thetaJ = j*2.0*math.pi/float(samplesXZ) n = exponentSide cos = math.cos(thetaJ) x = cos * scaleXZ * radius[0] z = math.pow( 1-math.pow(math.fabs(cos),n), 1.0/float(n) ) * math.copysign(1, math.sin(thetaJ)) * scaleXZ * radius[2] vertices.append( position + Vector3d(x,y,z) ) vertices.append( position + Vector3d(0,radius[1],0) ) vertices.append( position + Vector3d(0,-radius[1],0) ) faces = [] for i in range(0,(samplesY-2)*samplesXZ,samplesXZ) : for j in range(0,samplesXZ) : faces.append( (i+j, i+(j+1)%samplesXZ, i+samplesXZ+(j+1)%samplesXZ, i+samplesXZ+j) ) for i in range(0,samplesXZ) : base = (samplesY-2)*samplesXZ faces.append( ((i+1)%samplesXZ, i, (samplesY-1)*samplesXZ) ) faces.append( (base+i, base+(i+1)%samplesXZ, (samplesY-1)*samplesXZ+1) ) return create( vertices, faces, colour )
def interpret(self, value): value = self.basicInterpret(value) if isinstance(value, MathLib.Point3d) : return value else : return PyUtils.toPoint3d( value )
def interpret(self, value): value = self.basicInterpret(value) if isinstance(value, MathLib.Point3d): return value else: return PyUtils.toPoint3d(value)