Merge pull request #36 from FRC3476/shader-trajectories

Shader trajectories

core/assets/shaders/trajectory.frag

@@ -0,0 +1,13 @@
+#ifdef GL_ES
+#define LOWP lowp
+precision mediump float;
+#else
+#define LOWP
+#endif
+
+varying LOWP vec4 v_color;
+
+void main()
+{
+    gl_FragColor = v_color;
+}

core/assets/shaders/trajectory.vert

@@ -0,0 +1,23 @@
+// All components are in the range [0…1], including hue.
+vec3 hsv2rgb(vec3 c)
+{
+    vec4 K = vec4(1.0, 2.0 / 3.0, 1.0 / 3.0, 3.0);
+    vec3 p = abs(fract(c.xxx + K.xyz) * 6.0 - K.www);
+    return c.z * mix(K.xxx, clamp(p - K.xxx, 0.0, 1.0), c.y);
+}
+
+attribute vec2 a_position;
+attribute float a_speedPercent;
+
+uniform mat4 u_projTrans;
+uniform vec2 u_colorhv;
+
+varying vec4 v_color;
+
+void main()
+{
+    vec3 _hsvColor = vec3(u_colorhv.x, a_speedPercent * 0.9 + 0.1, u_colorhv.y);
+    v_color = vec4(hsv2rgb(_hsvColor), 0);
+    vec4 pos = vec4(a_position.xy, 0, 1);
+    gl_Position =  u_projTrans * pos;
+}

core/src/main/com/dacubeking/autobuilder/gui/AutoBuilder.java

@@ -4,6 +4,7 @@
 import com.badlogic.gdx.Gdx;
 import com.badlogic.gdx.Graphics;
 import com.badlogic.gdx.Input;
+import com.badlogic.gdx.assets.AssetManager;
 import com.badlogic.gdx.files.FileHandle;
 import com.badlogic.gdx.graphics.*;
 import com.badlogic.gdx.graphics.Cursor.SystemCursor;
@@ -327,7 +328,7 @@ private void draw() {
         //batch.flush();
 
         //Draw all the paths
-        origin.draw(shapeRenderer, cam);
+        origin.draw(shapeRenderer);
 
         double pathTime = 0;
         for (AbstractGuiItem guiItem : pathGui.guiItems) {
@@ -620,4 +621,11 @@ public void restoreState(Autonomous autonomous, boolean clearUndoHistory) {
         }
         undoHandler.forceCreateUndoState();
     }
+
+
+    private final @NotNull AssetManager assetManager = new AssetManager();
+
+    public @NotNull AssetManager getAssetManager() {
+        return assetManager;
+    }
 }

core/src/main/com/dacubeking/autobuilder/gui/pathing/MovablePointRenderer.java

@@ -12,8 +12,6 @@
 import com.dacubeking.autobuilder.gui.undo.UndoHandler;
 import org.jetbrains.annotations.NotNull;
 
-import static com.dacubeking.autobuilder.gui.AutoBuilder.POINT_SIZE;
-
 public class MovablePointRenderer extends PointRenderer {
 
 
@@ -45,7 +43,8 @@ public MovablePointRenderer(@NotNull Vector2 pos, @NotNull Color color, float ra
 
     public boolean update(@NotNull OrthographicCamera camera, @NotNull Vector3 mousePos, Vector3 mouseDiff) {
         if (Gdx.input.isButtonJustPressed(Input.Buttons.LEFT) || Gdx.input.isButtonJustPressed(Input.Buttons.RIGHT)) {
-            if (new Vector3(mousePos).sub(getRenderPos3()).len2() < Math.pow(Math.max(20 * camera.zoom, POINT_SIZE), 2)) {
+            if (new Vector3(mousePos).sub(getRenderPos3()).len2() <
+                    Math.pow(Math.max(20 * camera.zoom, AutoBuilder.getPointSize() * radius), 2)) {
                 if (Gdx.input.isButtonJustPressed(Input.Buttons.LEFT)) {
                     startPress.set(mousePos);
                     pressed = true;

core/src/main/com/dacubeking/autobuilder/gui/pathing/PointRenderer.java

@@ -1,7 +1,6 @@
 package com.dacubeking.autobuilder.gui.pathing;
 
 import com.badlogic.gdx.graphics.Color;
-import com.badlogic.gdx.graphics.OrthographicCamera;
 import com.badlogic.gdx.math.Vector2;
 import com.badlogic.gdx.math.Vector3;
 import com.dacubeking.autobuilder.gui.AutoBuilder;
@@ -12,11 +11,17 @@ public class PointRenderer {
     protected float x;
     protected float y;
     protected Color color;
+    protected float radius;
 
-    public PointRenderer(float x, float y, Color color) {
+    public PointRenderer(float x, float y, Color color, float radius) {
         this.x = x;
         this.y = y;
         this.color = color;
+        this.radius = radius;
+    }
+
+    public PointRenderer(float x, float y, Color color) {
+        this(x, y, color, 1);
     }
 
     public PointRenderer(@NotNull Vector2 pos, Color color) {
@@ -25,6 +30,14 @@ public PointRenderer(@NotNull Vector2 pos, Color color) {
         this.color = color;
     }
 
+    public PointRenderer(@NotNull Vector2 pos, Color color, float radius) {
+        this.x = pos.x;
+        this.y = pos.y;
+        this.color = color;
+        this.radius = radius;
+    }
+
+
     public PointRenderer(@NotNull Vector3 pos, Color color) {
         this.x = pos.x;
         this.y = pos.y;
@@ -36,9 +49,9 @@ public void move(float x, float y) {
         this.y = y;
     }
 
-    public void draw(@NotNull ShapeDrawer shape, @NotNull OrthographicCamera camera) {
+    public void draw(@NotNull ShapeDrawer shape) {
         float pointScaleFactor = AutoBuilder.getConfig().getPointScaleFactor();
-        shape.filledCircle(x * pointScaleFactor, y * pointScaleFactor, AutoBuilder.getPointSize(), color);
+        shape.filledCircle(x * pointScaleFactor, y * pointScaleFactor, AutoBuilder.getPointSize() * radius, color);
     }
 
 

core/src/main/com/dacubeking/autobuilder/gui/pathing/TrajectoryPathRenderer.java

@@ -2,8 +2,9 @@
 
 import com.badlogic.gdx.Gdx;
 import com.badlogic.gdx.Input.Buttons;
-import com.badlogic.gdx.graphics.Color;
-import com.badlogic.gdx.graphics.OrthographicCamera;
+import com.badlogic.gdx.graphics.*;
+import com.badlogic.gdx.graphics.VertexAttributes.Usage;
+import com.badlogic.gdx.graphics.glutils.ShaderProgram;
 import com.badlogic.gdx.math.Vector2;
 import com.badlogic.gdx.math.Vector3;
 import com.dacubeking.autobuilder.gui.AutoBuilder;
@@ -22,8 +23,8 @@
 import com.dacubeking.autobuilder.gui.pathing.pointclicks.ClosePoint;
 import com.dacubeking.autobuilder.gui.pathing.pointclicks.CloseTrajectoryPoint;
 import com.dacubeking.autobuilder.gui.undo.UndoHandler;
-import com.dacubeking.autobuilder.gui.util.CachedDrawingUtils;
 import com.dacubeking.autobuilder.gui.util.MathUtil;
+import com.dacubeking.autobuilder.gui.util.shaders.ShaderLoader;
 import com.dacubeking.autobuilder.gui.wpi.math.geometry.Pose2d;
 import com.dacubeking.autobuilder.gui.wpi.math.geometry.Rotation2d;
 import com.dacubeking.autobuilder.gui.wpi.math.spline.Spline;
@@ -37,7 +38,6 @@
 import com.dacubeking.autobuilder.gui.wpi.math.trajectory.constraint.TrajectoryConstraint;
 import org.jetbrains.annotations.NotNull;
 import org.jetbrains.annotations.Nullable;
-import space.earlygrey.shapedrawer.Drawing;
 import space.earlygrey.shapedrawer.ShapeDrawer;
 
 import java.io.Serializable;
@@ -54,6 +54,13 @@
 import java.util.concurrent.locks.ReentrantLock;
 
 public class TrajectoryPathRenderer extends PathRenderer implements MovablePointEventHandler, Serializable {
+
+    @NotNull private static final ShaderProgram trajectoryShader = ShaderLoader.loadShader("trajectory");
+    @NotNull private static final VertexAttributes shaderAttributes = new VertexAttributes(
+            new VertexAttribute(Usage.Generic, 2, ShaderProgram.POSITION_ATTRIBUTE),
+            new VertexAttribute(Usage.Generic, 1, "a_speedPercent")
+    );
+    private @Nullable Mesh mesh = null;
     @NotNull private final Color color;
     @NotNull private final List<Spline.ControlVector> controlVectors;
     @NotNull private final List<Rotation2d> rotation2dList;
@@ -81,6 +88,10 @@ public class TrajectoryPathRenderer extends PathRenderer implements MovablePoint
     private int robotPreviewIndex;
     @NotNull private final List<TrajectoryConstraint> constraints;
 
+    // Reused objects
+    private final float @NotNull [] colorHsv = new float[3];
+
+
     DecimalFormat df = new DecimalFormat("#.##");
 
     public TrajectoryPathRenderer(@NotNull Color color, @NotNull ControlVectorList pointList,
@@ -113,49 +124,50 @@ public void setPathChangeListener(@NotNull PathChangeListener pathChangeListener
         updatePath();
     }
 
-    private @Nullable Drawing cachedDrawing = null;
     private final @NotNull AtomicBoolean isDrawingCached = new AtomicBoolean(false);
 
     @Override
     protected void deleteRenderCache() {
         isDrawingCached.set(false);
-        cachedDrawing = null;
+        if (mesh != null) {
+            mesh.dispose();
+        }
+        mesh = null;
     }
 
     @Override
     public void render(@NotNull ShapeDrawer renderer, @NotNull OrthographicCamera cam) {
         Config config = AutoBuilder.getConfig();
         float pointScaleFactor = config.getPointScaleFactor();
+        //Get the first 2 points of the line at t = 0
+        lastPointLeft.set(0, -AutoBuilder.getLineThickness() / 2);
+        lastPointRight.set(0, AutoBuilder.getLineThickness() / 2);
 
-        if (cachedDrawing == null || !isDrawingCached.getAndSet(true)) {
-            cachedDrawing = CachedDrawingUtils.createNewDrawing(renderer);
-            CachedDrawingUtils.setDrawing(renderer, cachedDrawing);
-            //Get the first 2 points of the line at t = 0
-            lastPointLeft.set(0, -AutoBuilder.getLineThickness() / 2);
-            lastPointRight.set(0, AutoBuilder.getLineThickness() / 2);
-            if (trajectory != null) {
-                List<State> states = trajectory.getStates();
-                if (states.size() > 0) {
+        if (trajectory != null) {
+            List<State> states = trajectory.getStates();
+            if (states.size() > 0) {
+                if (!isDrawingCached.getAndSet(true) || mesh == null) {
+                    mesh = new Mesh(true, states.size() * 6 * 3, 0, shaderAttributes);
                     lastPointLeft.rotateRad((float) states.get(0).poseMeters.getRotation().getRadians());
                     lastPointRight.rotateRad((float) states.get(0).poseMeters.getRotation().getRadians());
 
                     lastPointLeft.add((float) states.get(0).poseMeters.getTranslation().getX() * pointScaleFactor,
                             (float) states.get(0).poseMeters.getTranslation().getY() * pointScaleFactor);
                     lastPointRight.add((float) states.get(0).poseMeters.getTranslation().getX() * pointScaleFactor,
                             (float) states.get(0).poseMeters.getTranslation().getY() * pointScaleFactor);
+                    double maxSpeed = AutoBuilder.getConfig().getPathingConfig().maxVelocityMetersPerSecond;
+                    float lastSpeedPercent = (float) (states.get(0).velocityMetersPerSecond / maxSpeed);
+
+
+                    int i = 0;
+                    float[] vertexData = new float[states.size() * 6 * 3];
+
 
                     for (State state : states) {
                         Pose2d cur = state.poseMeters;
 
                         //Use the speed of the path to determine its saturation
-                        double speed = Math.abs(state.velocityMetersPerSecond);
-                        float[] color = new float[3];
-                        this.color.toHsv(color);
-                        color[1] =
-                                (float) (0.9 * (speed / AutoBuilder.getConfig()
-                                        .getPathingConfig().maxVelocityMetersPerSecond) + 0.1);
-                        Color speedColor = new Color().fromHsv(color);
-                        speedColor.set(speedColor.r, speedColor.g, speedColor.b, 1);
+                        float speedPercent = (float) (Math.abs(state.velocityMetersPerSecond) / maxSpeed);
 
                         //Get the 2 points of the line at the current time
                         nextPointLeft.set(0, -AutoBuilder.getLineThickness() / 2);
@@ -169,36 +181,68 @@ public void render(@NotNull ShapeDrawer renderer, @NotNull OrthographicCamera ca
                         nextPointRight.add((float) cur.getTranslation().getX() * pointScaleFactor,
                                 (float) cur.getTranslation().getY() * pointScaleFactor);
 
-                        //Render the line
-                        renderer.setColor(speedColor);
-                        renderer.filledPolygon(new float[]{
-                                lastPointLeft.x, lastPointLeft.y,
-                                lastPointRight.x, lastPointRight.y,
-                                nextPointRight.x, nextPointRight.y,
-                                nextPointLeft.x, nextPointLeft.y
-                        });
 
+                        //Add the 4 vertices of the quad to the vertex data
+                        // Triangle 1
+                        vertexData[i++] = lastPointLeft.x;
+                        vertexData[i++] = lastPointLeft.y;
+                        vertexData[i++] = lastSpeedPercent;
+
+                        vertexData[i++] = lastPointRight.x;
+                        vertexData[i++] = lastPointRight.y;
+                        vertexData[i++] = lastSpeedPercent;
+
+
+                        vertexData[i++] = nextPointLeft.x;
+                        vertexData[i++] = nextPointLeft.y;
+                        vertexData[i++] = speedPercent;
+
+                        // Triangle 2
+                        vertexData[i++] = nextPointLeft.x;
+                        vertexData[i++] = nextPointLeft.y;
+                        vertexData[i++] = speedPercent;
+
+                        vertexData[i++] = lastPointRight.x;
+                        vertexData[i++] = lastPointRight.y;
+                        vertexData[i++] = lastSpeedPercent;
+
+                        vertexData[i++] = nextPointRight.x;
+                        vertexData[i++] = nextPointRight.y;
+                        vertexData[i++] = speedPercent;
+
+                        //Set the last point to the current point
                         lastPointLeft.set(nextPointLeft);
                         lastPointRight.set(nextPointRight);
+                        lastSpeedPercent = speedPercent;
                     }
+                    mesh.setVertices(vertexData);
                 }
-            }
+                renderer.getBatch().end();
+
 
-            CachedDrawingUtils.setDrawing(renderer, null);
-            assert cachedDrawing != null;
+                color.toHsv(colorHsv);
+
+                trajectoryShader.bind();
+                trajectoryShader.setUniformMatrix("u_projTrans", renderer.getBatch().getProjectionMatrix());
+                trajectoryShader.setUniformf("u_colorhv", colorHsv[0] / 360, colorHsv[2]);
+
+                mesh.render(trajectoryShader, GL20.GL_TRIANGLES);
+
+                renderer.getBatch().begin();
+            }
         }
-        cachedDrawing.draw();
+
 
         if (controlPoint != null) {
             PointRenderer selectedPoint = pointRenderList.get(selectionPointIndex);
             renderer.line(selectedPoint.getRenderPos2(), controlPoint.getRenderPos2(), Color.WHITE,
                     AutoBuilder.getLineThickness());
-            controlPoint.draw(renderer, cam);
+            controlPoint.draw(renderer);
 
             if (rotationPoint != null && config.isHolonomic()) {
                 renderer.line(selectedPoint.getRenderPos2(), rotationPoint.getRenderPos2(), Color.WHITE,
                         AutoBuilder.getLineThickness());
-                rotationPoint.draw(renderer, cam);
+                rotationPoint.draw(renderer);
             }
 
             Vector2 origin = selectedPoint.getRenderPos2();
@@ -237,14 +281,14 @@ public void render(@NotNull ShapeDrawer renderer, @NotNull OrthographicCamera ca
 
             if (i == selectionPointIndex) {
                 if (highlightPoint == null) {
-                    highlightPoint = new PointRenderer(pointRenderer.getPos2(), Color.WHITE);
+                    highlightPoint = new PointRenderer(pointRenderer.getPos2(), Color.WHITE, 1.4f);
                 } else {
                     highlightPoint.setPosition(pointRenderer.getPos2());
                 }
 
-                highlightPoint.draw(renderer, cam);
+                highlightPoint.draw(renderer);
             }
-            pointRenderer.draw(renderer, cam);
+            pointRenderer.draw(renderer);
         }
         //Reset the robot preview time so that it won't be visible in the next frame. (Requires that it is set again)
         robotPreviewTime = -1;

core/src/main/com/dacubeking/autobuilder/gui/util/shaders/ShaderLoader.java

@@ -0,0 +1,24 @@
+package com.dacubeking.autobuilder.gui.util.shaders;
+
+import com.badlogic.gdx.Gdx;
+import com.badlogic.gdx.assets.loaders.ShaderProgramLoader;
+import com.badlogic.gdx.graphics.glutils.ShaderProgram;
+import com.dacubeking.autobuilder.gui.AutoBuilder;
+import org.jetbrains.annotations.Contract;
+import org.jetbrains.annotations.NotNull;
+
+public final class ShaderLoader {
+
+    private ShaderLoader() {
+    }
+
+    private static final @NotNull ShaderProgramLoader.ShaderProgramParameter defaultShaderParameter =
+            new ShaderProgramLoader.ShaderProgramParameter();
+
+    @Contract("_ -> new")
+    public static @NotNull ShaderProgram loadShader(@NotNull String name) {
+        var assetManager = AutoBuilder.getInstance().getAssetManager();
+        return new ShaderProgram(Gdx.files.internal("shaders/" + name + ".vert"),
+                Gdx.files.internal("shaders/" + name + ".frag"));
+    }
+}