private void applyFrame(Mesh target, int frameIndex, float weight) {
PoseFrame frame = frames[frameIndex];
VertexBuffer pb = target.getBuffer(Type.Position);
for (int i = 0; i < frame.poses.length; i++) {
Pose pose = frame.poses[i];
float poseWeight = frame.weights[i] * weight;
pose.apply(poseWeight, (FloatBuffer) pb.getData());
}
// force to re-upload data to gpu
pb.updateData(pb.getData());
}
public Pose unmarshall(JsonUnmarshallerContext context) throws Exception {
AwsJsonReader reader = context.getReader();
if (!reader.isContainer()) {
reader.skipValue();
return null;
}
Pose pose = new Pose();
reader.beginObject();
while (reader.hasNext()) {
String name = reader.nextName();
if (name.equals("Roll")) {
pose.setRoll(FloatJsonUnmarshaller.getInstance().unmarshall(context));
} else if (name.equals("Yaw")) {
pose.setYaw(FloatJsonUnmarshaller.getInstance().unmarshall(context));
} else if (name.equals("Pitch")) {
pose.setPitch(FloatJsonUnmarshaller.getInstance().unmarshall(context));
} else {
reader.skipValue();
}
}
reader.endObject();
return pose;
}
/**
* Calculate the range of a robot to the nearest wall
*
* @param pose the pose of the robot
* @return the range or -1 if not in range
*/
public float range(Pose pose) {
Line l =
new Line(
pose.getX(),
pose.getY(),
pose.getX() + 254f * (float) Math.cos(Math.toRadians(pose.getHeading())),
pose.getY() + 254f * (float) Math.sin(Math.toRadians(pose.getHeading())));
Line rl = null;
for (int i = 0; i < lines.length; i++) {
Point p = lines[i].intersectsAt(l);
if (p == null) continue; // Does not intersect
Line tl = new Line(pose.getX(), pose.getY(), p.x, p.y);
// If the range line intersects more than one map line
// then take the shortest distance.
if (rl == null || tl.length() < rl.length()) rl = tl;
}
return (rl == null ? -1 : rl.length());
}
