mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-21 09:31:20 -08:00
Use circle buffer to get previous rotations
Should help with performance.
This commit is contained in:
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);
|
||||
}
|
||||
|
||||
Reference in new issue
Block a user