Use circle buffer to get previous rotations

Should help with performance.
This commit is contained in:
Louka committed 2022-03-27 23:58:23 -04:00
1 parent 98068232a6
commit 4d2083df27
2 files changed
+115 -8

No files matched your search

@@ -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<E>
extends AbstractList<E> implements RandomAccess {
private final int n; // buffer length
private final List<E> buf; // a List implementing RandomAccess
private int head = 0;
private int tail = 0;
public CircularArrayList(int capacity) {
n = capacity + 1;
buf = new ArrayList<E>(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;
}
}
@@ -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<Quaternion> previousRots = new LinkedList<Quaternion>();
protected CircularArrayList<Quaternion> 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<Quaternion>(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);
}