mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-21 09:31:20 -08:00
Basic interpolation and extrapolation
only the logic. Need to make config and GUI
This commit is contained in:
1 parent
dadb0bb378
commit
4263e86189
1 file changed
+21
-1
@@ -1,5 +1,6 @@
|
||||
package dev.slimevr.vr.trackers;
|
||||
|
||||
import java.util.LinkedList;
|
||||
import com.jme3.math.FastMath;
|
||||
import com.jme3.math.Quaternion;
|
||||
import com.jme3.math.Vector3f;
|
||||
@@ -18,6 +19,7 @@ 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 TrackerMountingRotation mounting = null;
|
||||
protected TrackerStatus status = TrackerStatus.OK;
|
||||
protected final int trackerId;
|
||||
@@ -101,6 +103,10 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
|
||||
calculateLiveMagnetometerCorrection();
|
||||
}
|
||||
}
|
||||
previousRots.addLast(new Quaternion(rotQuaternion));
|
||||
if(previousRots.size() > 2){ // How far back in will getFilteredRotation go to get oldRot
|
||||
previousRots.removeFirst();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -116,11 +122,25 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
|
||||
|
||||
@Override
|
||||
public boolean getRotation(Quaternion store) {
|
||||
store.set(rotQuaternion);
|
||||
float filterFactor = 1f; // 1 = do nothing, >0<1 = interpolation, >1 = exterpolation
|
||||
store.set(getFilteredRotation(rotQuaternion, previousRots.getFirst(), filterFactor));
|
||||
//correction.mult(store, store); // Correction is not used now to prevent accidental errors while debugging other things
|
||||
store.multLocal(rotAdjust);
|
||||
return true;
|
||||
}
|
||||
|
||||
// Original code: https://answers.unity.com/questions/168779/extrapolating-quaternion-rotation.html
|
||||
Quaternion getFilteredRotation(Quaternion latestRot, Quaternion oldRot, float factor){
|
||||
Quaternion rot = oldRot.mult(latestRot.inverse());
|
||||
while (factor > 1){
|
||||
oldRot = latestRot;
|
||||
latestRot = latestRot.mult(rot);
|
||||
factor -= 1;
|
||||
}
|
||||
Quaternion filteredQuaternion = new Quaternion();
|
||||
filteredQuaternion = filteredQuaternion.slerp(oldRot, latestRot, factor);
|
||||
return filteredQuaternion;
|
||||
}
|
||||
|
||||
public void getCorrection(Quaternion store) {
|
||||
store.set(correction);
|
||||
|
||||
Reference in new issue
Block a user