From 4d2083df272fc87ce2137ef7a5cbf2d293b4f438 Mon Sep 17 00:00:00 2001 From: Louka Date: Sun, 27 Mar 2022 23:58:23 -0400 Subject: [PATCH] Use circle buffer to get previous rotations Should help with performance. --- .../vr/trackers/CircularArrayList.java | 107 ++++++++++++++++++ .../dev/slimevr/vr/trackers/IMUTracker.java | 16 +-- 2 files changed, 115 insertions(+), 8 deletions(-) create mode 100644 src/main/java/dev/slimevr/vr/trackers/CircularArrayList.java diff --git a/src/main/java/dev/slimevr/vr/trackers/CircularArrayList.java b/src/main/java/dev/slimevr/vr/trackers/CircularArrayList.java new file mode 100644 index 000000000..277f3d61d --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/CircularArrayList.java @@ -0,0 +1,107 @@ +package dev.slimevr.vr.trackers; + +import java.util.AbstractList; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import java.util.RandomAccess; + +/** + * If you use this code, please consider notifying isak at du-preez dot com + * with a brief description of your application. + * + * This is free and unencumbered software released into the public domain. + * Anyone is free to copy, modify, publish, use, compile, sell, or + * distribute this software, either in source code form or as a compiled + * binary, for any purpose, commercial or non-commercial, and by any + * means. + */ + +public class CircularArrayList + extends AbstractList implements RandomAccess { + + private final int n; // buffer length + private final List buf; // a List implementing RandomAccess + private int head = 0; + private int tail = 0; + + public CircularArrayList(int capacity) { + n = capacity + 1; + buf = new ArrayList(Collections.nCopies(n, (E) null)); + } + + public int capacity() { + return n - 1; + } + + private int wrapIndex(int i) { + int m = i % n; + if (m < 0) { // java modulus can be negative + m += n; + } + return m; + } + + // This method is O(n) but will never be called if the + // CircularArrayList is used in its typical/intended role. + private void shiftBlock(int startIndex, int endIndex) { + assert (endIndex > startIndex); + for (int i = endIndex - 1; i >= startIndex; i--) { + set(i + 1, get(i)); + } + } + + @Override + public int size() { + return tail - head + (tail < head ? n : 0); + } + + @Override + public E get(int i) { + if (i < 0 || i >= size()) { + throw new IndexOutOfBoundsException(); + } + return buf.get(wrapIndex(head + i)); + } + + @Override + public E set(int i, E e) { + if (i < 0 || i >= size()) { + throw new IndexOutOfBoundsException(); + } + return buf.set(wrapIndex(head + i), e); + } + + @Override + public void add(int i, E e) { + int s = size(); + if (s == n - 1) { + throw new IllegalStateException( + "CircularArrayList is filled to capacity. " + + "(You may want to remove from front" + + " before adding more to back.)"); + } + if (i < 0 || i > s) { + throw new IndexOutOfBoundsException(); + } + tail = wrapIndex(tail + 1); + if (i < s) { + shiftBlock(i, s); + } + set(i, e); + } + + @Override + public E remove(int i) { + int s = size(); + if (i < 0 || i >= s) { + throw new IndexOutOfBoundsException(); + } + E e = get(i); + if (i > 0) { + shiftBlock(0, i); + } + head = wrapIndex(head + 1); + return e; + } +} \ No newline at end of file diff --git a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java index 4e0ffb5a1..05a202c78 100644 --- a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java @@ -1,6 +1,5 @@ package dev.slimevr.vr.trackers; -import java.util.LinkedList; import com.jme3.math.FastMath; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; @@ -20,9 +19,9 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { public final Quaternion rotMagQuaternion = new Quaternion(); public final Quaternion rotAdjust = new Quaternion(); protected final Quaternion correction = new Quaternion(); - protected LinkedList previousRots = new LinkedList(); + protected CircularArrayList previousRots; private final Quaternion buffQuat = new Quaternion(); - public float movementFilterTickCount = 0; + public int movementFilterTickCount = 0; public float movementFilterAmount = 1f; protected TrackerMountingRotation mounting = null; protected TrackerStatus status = TrackerStatus.OK; @@ -110,6 +109,7 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { movementFilterAmount = 1f; movementFilterTickCount = 0; } + previousRots = new CircularArrayList(movementFilterTickCount); } public TrackerMountingRotation getMountingRotation() { return mounting; @@ -135,9 +135,9 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { } } if(movementFilterTickCount != 0){ - previousRots.addLast(rotQuaternion.clone()); - if(previousRots.size() > movementFilterTickCount){ - previousRots.removeFirst(); + previousRots.add(rotQuaternion.clone()); + if(previousRots.size() > movementFilterTickCount - 1){ + previousRots.remove(0); } } } @@ -155,8 +155,8 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { @Override public boolean getRotation(Quaternion store) { - if(movementFilterTickCount > 0 && movementFilterAmount != 1 && previousRots.getFirst() != null){ - buffQuat.set(previousRots.getFirst()); + if(movementFilterTickCount > 0 && movementFilterAmount != 1 && previousRots.size() > 0){ + buffQuat.set(previousRots.get(0)); buffQuat.slerp(rotQuaternion, movementFilterAmount); store.set(buffQuat); }