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()
