Skip to content

Commit d6f771e

Browse files
ButterscotchVButterscotchV
authored
BVH rewrite (#1542)
1 parent 3aeb056 commit d6f771e

File tree

2 files changed

+92
-101
lines changed

2 files changed

+92
-101
lines changed

‎server/core/src/main/java/dev/slimevr/posestreamer/BVHFileStream.kt‎

Lines changed: 86 additions & 95 deletions
Original file line numberDiff line numberDiff line change
@@ -20,28 +20,19 @@ class BVHFileStream : PoseDataStream {
2020
private var frameCount: Long = 0
2121
private var frameCountOffset: Long = 0
2222

23-
constructor(outputStream: OutputStream) : super(outputStream) {
24-
writer = BufferedWriter(OutputStreamWriter(outputStream), 4096)
25-
}
26-
27-
constructor(outputStream: OutputStream, bvhSettings: BVHSettings) : this(outputStream) {
23+
constructor(outputStream: OutputStream, bvhSettings: BVHSettings = BVHSettings.BLENDER) : super(outputStream) {
2824
this.bvhSettings = bvhSettings
29-
}
30-
31-
constructor(file: File) : super(file) {
3225
writer = BufferedWriter(OutputStreamWriter(outputStream), 4096)
3326
}
3427

35-
constructor(file: File, bvhSettings: BVHSettings) : this(file) {
28+
constructor(file: File, bvhSettings: BVHSettings = BVHSettings.BLENDER) : super(file) {
3629
this.bvhSettings = bvhSettings
37-
}
38-
39-
constructor(file: String) : super(file) {
4030
writer = BufferedWriter(OutputStreamWriter(outputStream), 4096)
4131
}
4232

43-
constructor(file: String, bvhSettings: BVHSettings) : this(file) {
33+
constructor(file: String, bvhSettings: BVHSettings = BVHSettings.BLENDER) : super(file) {
4434
this.bvhSettings = bvhSettings
35+
writer = BufferedWriter(OutputStreamWriter(outputStream), 4096)
4536
}
4637

4738
private fun getBufferedFrameCount(frameCount: Long): String {
@@ -51,83 +42,108 @@ class BVHFileStream : PoseDataStream {
5142
return if (bufferCount > 0) frameString + StringUtils.repeat(' ', bufferCount) else frameString
5243
}
5344

54-
private fun isEndBone(bone: Bone?): Boolean = bone == null || (!bvhSettings.shouldWriteEndNodes() && bone.children.isEmpty())
45+
private fun internalNavigateSkeleton(
46+
bone: Bone,
47+
header: (bone: Bone, lastBone: Bone?, invertParentRot: Quaternion, distance: Int, hasBranch: Boolean, isParent: Boolean) -> Unit,
48+
footer: (distance: Int) -> Unit,
49+
lastBone: Bone? = null,
50+
invertParentRot: Quaternion = Quaternion.IDENTITY,
51+
distance: Int = 0,
52+
isParent: Boolean = false,
53+
) {
54+
val parent = bone.parent
55+
// If we're visiting the parents or at root, continue to the next parent
56+
val visitParent = (isParent || lastBone == null) && parent != null
5557

56-
@Throws(IOException::class)
57-
private fun writeBoneHierarchy(bone: Bone?, level: Int = 0) {
58-
// Treat null as bone. This allows for simply writing empty end bones
59-
val isEndBone = isEndBone(bone)
60-
61-
// Don't write end sites at populated bones, BVH parsers don't like that
62-
// Ex case caught: `joint{ joint{ end }, end, end }` outputs `joint{ end
63-
// }` instead
64-
// Ex case let through: `joint{ end }`
65-
val isSingleChild = (bone?.parent?.children?.size ?: 0) <= 1
66-
if (isEndBone && !isSingleChild) {
67-
return
58+
val children = bone.children
59+
val childCount = children.size - (if (isParent) 1 else 0)
60+
61+
val hasBranch = visitParent || childCount > 0
62+
63+
header(bone, lastBone, invertParentRot, distance, hasBranch, isParent)
64+
65+
if (hasBranch) {
66+
// Cache this inverted rotation to reduce computation for each branch
67+
val thisInvertRot = bone.getGlobalRotation().inv()
68+
69+
if (visitParent) {
70+
internalNavigateSkeleton(parent, header, footer, bone, thisInvertRot, distance + 1, true)
71+
}
72+
73+
for (child in children) {
74+
// If we're a parent, ignore the child
75+
if (isParent && child == lastBone) continue
76+
internalNavigateSkeleton(child, header, footer, bone, thisInvertRot, distance + 1, false)
77+
}
6878
}
6979

70-
val indentLevel = StringUtils.repeat("\t", level)
80+
footer(distance)
81+
}
82+
83+
private fun navigateSkeleton(
84+
root: Bone,
85+
header: (bone: Bone, lastBone: Bone?, invertParentRot: Quaternion, distance: Int, hasBranch: Boolean, isParent: Boolean) -> Unit,
86+
footer: (distance: Int) -> Unit = {},
87+
) {
88+
internalNavigateSkeleton(root, header, footer)
89+
}
90+
91+
private fun writeBoneDefHeader(bone: Bone?, lastBone: Bone?, invertParentRot: Quaternion, distance: Int, hasBranch: Boolean, isParent: Boolean) {
92+
val indentLevel = StringUtils.repeat("\t", distance)
7193
val nextIndentLevel = indentLevel + "\t"
7294

7395
// Handle ends
74-
if (isEndBone) {
75-
writer.write(indentLevel + "End Site\n")
96+
if (bone == null) {
97+
writer.write("${indentLevel}End Site\n")
7698
} else {
7799
writer
78-
.write((if (level > 0) indentLevel + "JOINT " else "ROOT ") + bone!!.boneType + "\n")
100+
.write("${indentLevel}${if (distance > 0) "JOINT" else "ROOT"} ${bone.boneType}\n")
79101
}
80102
writer.write("$indentLevel{\n")
81103

82-
// Ignore the root offset and original root offset
83-
if (level > 0 && bone != null && bone.parent != null) {
84-
val offsetScale = bvhSettings.offsetScale
85-
writer
86-
.write(
87-
(
88-
nextIndentLevel +
89-
"OFFSET " +
90-
0 +
91-
" "
92-
) + -bone.parent!!.length * offsetScale + " " +
93-
0 +
94-
"\n",
95-
)
104+
// Ignore the root and endpoint offsets
105+
if (bone != null && lastBone != null) {
106+
writer.write(
107+
"${nextIndentLevel}OFFSET 0.0 ${(if (isParent) lastBone.length else -lastBone.length) * bvhSettings.offsetScale} 0.0\n",
108+
)
96109
} else {
97-
writer.write(nextIndentLevel + "OFFSET 0.0 0.0 0.0\n")
110+
writer.write("${nextIndentLevel}OFFSET 0.0 0.0 0.0\n")
98111
}
99112

100-
// Handle ends
101-
if (!isEndBone) {
113+
// Define channels
114+
if (bone != null) {
102115
// Only give position for root
103-
if (level > 0) {
104-
writer.write(nextIndentLevel + "CHANNELS 3 Zrotation Xrotation Yrotation\n")
116+
if (lastBone != null) {
117+
writer.write("${nextIndentLevel}CHANNELS 3 Zrotation Xrotation Yrotation\n")
105118
} else {
106-
writer
107-
.write(
108-
nextIndentLevel +
109-
"CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation\n",
110-
)
119+
writer.write(
120+
"${nextIndentLevel}CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation\n",
121+
)
111122
}
112123

113-
// If the bone has children
114-
if (bone!!.children.isNotEmpty()) {
115-
for (childBone in bone.children) {
116-
writeBoneHierarchy(childBone, level + 1)
117-
}
118-
} else {
119-
// Write an empty end bone
120-
writeBoneHierarchy(null, level + 1)
124+
// Write an empty end bone if there are no branches
125+
// We use null for convenience and treat it as an end node (no bone)
126+
if (!hasBranch) {
127+
val endDistance = distance + 1
128+
writeBoneDefHeader(null, bone, Quaternion.IDENTITY, endDistance, false, false)
129+
writeBoneDefFooter(endDistance)
121130
}
122131
}
132+
}
123133

124-
writer.write("$indentLevel}\n")
134+
private fun writeBoneDefFooter(level: Int) {
135+
// Closing bracket
136+
writer.write("${StringUtils.repeat("\t", level)}}\n")
137+
}
138+
139+
private fun writeSkeletonDef(rootBone: Bone) {
140+
navigateSkeleton(rootBone, ::writeBoneDefHeader, ::writeBoneDefFooter)
125141
}
126142

127143
@Throws(IOException::class)
128144
override fun writeHeader(skeleton: HumanSkeleton, streamer: PoseStreamer) {
129145
writer.write("HIERARCHY\n")
130-
writeBoneHierarchy(skeleton.headBone)
146+
writeSkeletonDef(skeleton.getBone(bvhSettings.rootBone))
131147

132148
writer.write("MOTION\n")
133149
writer.write("Frames: ")
@@ -145,41 +161,19 @@ class BVHFileStream : PoseDataStream {
145161
writer.write("Frame Time: ${streamer.frameInterval}\n")
146162
}
147163

148-
@Throws(IOException::class)
149-
private fun writeBoneHierarchyRotation(bone: Bone, inverseRootRot: Quaternion?) {
150-
var rot = bone.getGlobalRotation()
151-
152-
// Adjust to local rotation
153-
if (inverseRootRot != null) {
154-
rot = inverseRootRot * rot
155-
}
156-
157-
// Pitch (X), Yaw (Y), Roll (Z)
164+
private fun writeBoneRot(bone: Bone, lastBone: Bone?, invertParentRot: Quaternion, distance: Int, hasBranch: Boolean, isParent: Boolean) {
165+
val rot = invertParentRot * bone.getGlobalRotation()
158166
val angles = rot.toEulerAngles(EulerOrder.ZXY)
159167

160168
// Output in order of roll (Z), pitch (X), yaw (Y) (extrinsic)
169+
// Assume spacing is needed at the start (we start with position with no following space)
161170
writer
162-
.write("${angles.z * FastMath.RAD_TO_DEG} ${angles.x * FastMath.RAD_TO_DEG} ${angles.y * FastMath.RAD_TO_DEG}")
163-
164-
// Get inverse rotation for child local rotations
165-
if (bone.children.isNotEmpty()) {
166-
val inverseRot = bone.getGlobalRotation().inv()
167-
for (childBode in bone.children) {
168-
if (isEndBone(childBode)) {
169-
// If it's an end bone, skip
170-
continue
171-
}
172-
173-
// Add spacing
174-
writer.write(" ")
175-
writeBoneHierarchyRotation(childBode, inverseRot)
176-
}
177-
}
171+
.write(" ${angles.z * FastMath.RAD_TO_DEG} ${angles.x * FastMath.RAD_TO_DEG} ${angles.y * FastMath.RAD_TO_DEG}")
178172
}
179173

180174
@Throws(IOException::class)
181175
override fun writeFrame(skeleton: HumanSkeleton) {
182-
val rootBone = skeleton.headBone
176+
val rootBone = skeleton.getBone(bvhSettings.rootBone)
183177

184178
val rootPos = rootBone.getPosition()
185179

@@ -188,10 +182,7 @@ class BVHFileStream : PoseDataStream {
188182
writer
189183
.write("${rootPos.x * positionScale} ${rootPos.y * positionScale} ${rootPos.z * positionScale}")
190184

191-
// Add spacing
192-
writer.write(" ")
193-
writeBoneHierarchyRotation(rootBone, null)
194-
185+
navigateSkeleton(rootBone, ::writeBoneRot)
195186
writer.newLine()
196187

197188
frameCount++

‎server/core/src/main/java/dev/slimevr/posestreamer/BVHSettings.kt‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,18 +1,20 @@
11
package dev.slimevr.posestreamer
22

3+
import dev.slimevr.tracking.processor.BoneType
4+
35
class BVHSettings {
46
var offsetScale: Float = 100f
57
private set
68
var positionScale: Float = 100f
79
private set
8-
private var writeEndNodes = false
10+
var rootBone: BoneType = BoneType.HIP
11+
private set
912

1013
constructor()
1114

1215
constructor(source: BVHSettings) {
1316
this.offsetScale = source.offsetScale
1417
this.positionScale = source.positionScale
15-
this.writeEndNodes = source.writeEndNodes
1618
}
1719

1820
fun setOffsetScale(offsetScale: Float): BVHSettings {
@@ -25,10 +27,8 @@ class BVHSettings {
2527
return this
2628
}
2729

28-
fun shouldWriteEndNodes(): Boolean = writeEndNodes
29-
30-
fun setWriteEndNodes(writeEndNodes: Boolean): BVHSettings {
31-
this.writeEndNodes = writeEndNodes
30+
fun setRootBone(rootBone: BoneType): BVHSettings {
31+
this.rootBone = rootBone
3232
return this
3333
}
3434

0 commit comments

Comments
 (0)