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