def dowrite(self): self._listchildmeshes() filename = self.basename+".class" with self._fileopenwrite(filename) as f: self.f = f meshptrs = [] physidx = 0 for m in self.meshes: def _getparentphys(m): ph = None mesh = m while mesh and not ph: for phys in self.bodies: meshes = list(filter(None, [ch == mesh for ch in phys.childmeshes])) if len(meshes) == 1: if not ph: ph = phys else: print("Error: both phys %s and %s has mesh refs to %s." % (ph.getFullName(), phys.getFullName(), mesh.getFullName())) print(ph.childmeshes, meshes) sys.exit(3) elif len(meshes) > 1: print("Error: phys %s has multiple mesh children refs to %s." % (phys.getFullName(), mesh.getFullName())) sys.exit(3) mesh = mesh.getParent() if not ph: print("Warning: mesh %s is not attached to any physics object!" % m.getFullName()) return ph phys = _getparentphys(m) if not phys: continue phys.writecount = 1 m.writecount += 1 tm = m.get_world_transform() tp = phys.get_world_transform() tmt, tmr, _ = tm.decompose() tpt, tpr, _ = tp.decompose() q = quat(tpr.inverse()).normalize() p = tmt-tpt p = q.toMat4() * vec4(p[:]) q = quat(tpr.inverse() * tmr).normalize() p = p[0:3] t = self._normalizexform(q[:]+p[:]) physidx = self.bodies.index(phys) meshptrs += [(CHUNK_CLASS_PHYS_MESH, PhysMeshPtr(physidx, m.meshbasename, t, m.mat))] data = ( CHUNK_CLASS, ( (CHUNK_CLASS_PHYSICS, self.basename), (CHUNK_CLASS_MESH_LIST, meshptrs), ) ) #pprint.pprint(data) self._writechunk(data) self._addfeat("class:classes", 1) self._verifywritten("physics->mesh links", self.meshes) self._addfeat("file:files", 1)
def _normalizexform(self, xform): "Normalize the hard way. Most importantly the orientation." ## self._clampdata(xform, 4) ## xform[:4] = quat(xform[:4]).normalize()[:] ## m = quat(xform[:4]).toMat4() ## self._clampdata(m.mlist, 16) ## xform[:4] = quat().fromMat(m).normalize()[:] self._clampdata(xform, 4) xform[:4] = quat(xform[:4]).normalize()[:] for x in range(4, 7): xround = round(xform[x], 1) if math.fabs(xround-xform[x]) < 1e-3: xform[x] = xround return xform
def _normalizexform(self, xform): "Normalize the hard way. Most importantly the orientation." ## self._clampdata(xform, 4) ## xform[:4] = quat(xform[:4]).normalize()[:] ## m = quat(xform[:4]).toMat4() ## self._clampdata(m.mlist, 16) ## xform[:4] = quat().fromMat(m).normalize()[:] self._clampdata(xform, 4) xform[:4] = quat(xform[:4]).normalize()[:] for x in range(4, 7): xround = round(xform[x], 1) if math.fabs(xround - xform[x]) < 1e-3: xform[x] = xround return xform
def solveArrays(xArrays,yArrays): """ # TRIAD (1st try): # Does not work :( Asub = [] for x in range(0,3): obs = np.array([float(f) for f in yArrays[x][1:]]) star_id = yArrays[x][0] star_data = xArrays[int(star_id)] star_loc = np.array([float(f) for f in star_data[:3]]) bi = np.reshape(obs,(-1, 3)) ri = np.reshape(star_loc,(-1, 3)) Asub.append(ri.T * bi) A = Asub[0]+Asub[1]+Asub[2] """ # SVD: # Based on https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19990104598.pdf # Slides 1 and 4 B = np.array([[0.,0.,0.],[0.,0.,0.],[0.,0.,0.]]) for obs in yArrays: star_id = obs[0] obs = np.array([float(f) for f in obs[1:]]) star_data = xArrays[int(star_id)] star_loc = np.array([float(f) for f in star_data[:3]]) star_mag = float(star_data[3]) ai = star_mag bi = np.reshape(obs,(-1, 3)) ri = np.reshape(star_loc,(-1, 3)) print(f"Observed star {star_id} at {bi}. Catalog vector: {ri}"); B += ai * bi * ri.T u, s, vh = np.linalg.svd(B, full_matrices=True) C = [[1, 0, 0], [0, 1, 0], [0, 0, np.linalg.det(u) * np.linalg.det(vh)]] A = np.matmul(np.matmul(u, C), vh) qx, qy, qz, qw = quat(A) q = Quaternion(qw, qx, qy, qz).normalised print(f"Attitude: (x:{q.x}, y:{q.y}, z:{q.z}, w:{q.w})") return q
def creategfxobject(vertices,indices): initgfxmesh(quat(1,0,0,0),vec3(0,0,0),vertices,indices) createobject()
def dowrite(self): self._listchildmeshes() filename = self.basename + ".class" with self._fileopenwrite(filename) as f: self.f = f meshptrs = [] physidx = 0 for m in self.meshes: def _getparentphys(m): ph = None mesh = m while mesh and not ph: for phys in self.bodies: meshes = list( filter(None, [ch == mesh for ch in phys.childmeshes])) if len(meshes) == 1: if not ph: ph = phys else: print( "Error: both phys %s and %s has mesh refs to %s." % (ph.getFullName(), phys.getFullName(), mesh.getFullName())) print(ph.childmeshes, meshes) sys.exit(3) elif len(meshes) > 1: print( "Error: phys %s has multiple mesh children refs to %s." % (phys.getFullName(), mesh.getFullName())) sys.exit(3) mesh = mesh.getParent() if not ph: print( "Warning: mesh %s is not attached to any physics object!" % m.getFullName()) return ph phys = _getparentphys(m) if not phys: continue phys.writecount = 1 m.writecount += 1 tm = m.get_world_transform() tp = phys.get_world_transform() tmt, tmr, _ = tm.decompose() tpt, tpr, _ = tp.decompose() q = quat(tpr.inverse()).normalize() p = tmt - tpt p = q.toMat4() * vec4(p[:]) q = quat(tpr.inverse() * tmr).normalize() p = p[0:3] t = self._normalizexform(q[:] + p[:]) physidx = self.bodies.index(phys) meshptrs += [(CHUNK_CLASS_PHYS_MESH, PhysMeshPtr(physidx, m.meshbasename, t, m.mat))] data = (CHUNK_CLASS, ( (CHUNK_CLASS_PHYSICS, self.basename), (CHUNK_CLASS_MESH_LIST, meshptrs), )) #pprint.pprint(data) self._writechunk(data) self._addfeat("class:classes", 1) self._verifywritten("physics->mesh links", self.meshes) self._addfeat("file:files", 1)
velocity = np.array([0, 0, 0]) position = np.array([0, 0, 0]) time_sample = time.time() j = 0 flag3 = 0 yaw_offset = 0 while (time.time() - time1 < 25): i = i + 1 data = udp.update() hash1, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, yaw, pitch, roll, yaw1, yaw2 = data.split( ) acceleration = np.array([float(acc_x), float(acc_y), float(acc_z)]) angular = np.array([float(gyro_x), float(gyro_y), float(gyro_z)]) quater1 = quat.quat(float(roll), float(pitch), float(yaw)) acc_t = gpd.update_d(acceleration, angular, quater1) ans = gpd.stance_con() #print(yaw) ### if stance condition is ended then set velocity to zero ### This is ZERO VELOCITY UPDATE if (prev == 1 and ans == 0 and c**t > 20): c**t = 0 steps = steps + 1 Threshold = gpd.reset() print("end of stance : step number : Threshold", steps, Threshold) flag3 = 0 velocity = np.zeros(3) if (prev == 0 and ans == 1): print("start of stance")
def main_loop(self, realdata): planet = Planet(200, 1, 0) pygame.init() screen = pygame.display.set_mode((planet.max_row, planet.row_count), HWSURFACE) pygame.display.set_caption("Star Map") background = pygame.Surface(screen.get_size()) background.fill((128, 128, 128)) def in_bounds(x, y): return x > planet.row_offsets[y] and x < planet.row_offsets[y] + planet.row_lengths[y] background.lock() for y in range(0, screen.get_height()): for x in range(0, screen.get_width()): if in_bounds(x, y): background.set_at((x, y), (0, 0, 0)) background.unlock() def color(index): if index < 0.25: s = int(255 * (index + 0.5) / 0.75) return s, s, 255 elif index < 1: return 255, 255, int(255 * (1 - index) / 0.75) else: return 255, int(255 * (2 - min(2, index))), 0 def magnitude(color, magnitude, distance): apparent = magnitude - 5 * (log10(distance / 3.26) + 1) scale = 1 - (apparent + 40) / 45.0 scale = min(1, scale) return [int(c * scale) for c in color] data = StarData() if realdata else RandomStars() rot_th, rot_u = data.rotation rot = quat(rot_u[0] * sin(rot_th / 2), rot_u[1] * sin(rot_th / 2), rot_u[2] * sin(rot_th / 2), cos(rot_th / 2)) for s in data.stars: r, theta, phi = s.location sin_th = sin(theta) v = sin_th * cos(phi), sin_th * sin(phi), cos(theta) q = quat(0, *v) v = ((rot * q) / rot).q[1:4] x, y = planet.vector_to_xy(v) background.set_at((int(x), int(y)), magnitude(color(s.color), s.magnitude, r)) screen.blit(background, (0, 0)) done = False while not done: for event in pygame.event.get(): if event.type == QUIT: done = True elif event.type == KEYDOWN: if event.key == K_ESCAPE: done = True pygame.display.flip()
def main_loop(self, realdata): planet = Planet(200,1,0) pygame.init() screen = pygame.display.set_mode((planet.max_row,planet.row_count), HWSURFACE) pygame.display.set_caption('Star Map') background = pygame.Surface(screen.get_size()) background.fill((128,128,128)) def in_bounds(x,y): return (x > planet.row_offsets[y] and x < planet.row_offsets[y] + planet.row_lengths[y]) background.lock() for y in range(0, screen.get_height()): for x in range(0, screen.get_width()): if in_bounds(x,y): background.set_at((x,y), (0,0,0)) background.unlock() def color(index): if index < 0.25: s = int(255 * (index + 0.5)/0.75) return s, s, 255 elif index < 1: return 255, 255, int(255 * (1 - index)/0.75) else: return 255, int(255 * (2 - min(2, index))), 0 def magnitude(color, magnitude, distance): apparent = magnitude - 5 * (log10(distance/3.26) + 1) scale = 1 - (apparent + 40)/45.0 scale = min(1, scale) return [int(c * scale) for c in color] data = StarData() if realdata else RandomStars() rot_th, rot_u = data.rotation rot = quat(rot_u[0] * sin(rot_th/2), rot_u[1] * sin(rot_th/2), rot_u[2] * sin(rot_th/2), cos(rot_th/2)) for s in data.stars: r, theta, phi = s.location sin_th = sin(theta) v = sin_th * cos(phi), sin_th * sin(phi), cos(theta) q = quat(0, *v) v = ((rot*q)/rot).q[1:4] x,y = planet.vector_to_xy(v) background.set_at((int(x),int(y)), magnitude(color(s.color), s.magnitude, r)) screen.blit(background, (0,0)) done = False while not done: for event in pygame.event.get(): if event.type == QUIT: done = True elif event.type == KEYDOWN: if event.key == K_ESCAPE: done = True pygame.display.flip()