mirror of
https://github.com/AxioDL/metaforce.git
synced 2025-05-13 16:31:21 +00:00
7f7a18a708
Avoids indirect inclusions where applicable and includes the necessary headers as used by the interface. This way, it prevents code from failing to compile due to changes in other header inclusions.
463 lines
15 KiB
C++
463 lines
15 KiB
C++
#include "DataSpec/DNACommon/ANIM.hpp"
|
|
|
|
#include <cfloat>
|
|
#include <cmath>
|
|
#include <cstring>
|
|
|
|
#include <hecl/hecl.hpp>
|
|
#include <zeus/Global.hpp>
|
|
#include <zeus/Math.hpp>
|
|
|
|
#define DUMP_KEYS 0
|
|
|
|
#if DUMP_KEYS
|
|
#include <cstdio>
|
|
#include <fmt/format.h>
|
|
#endif
|
|
|
|
namespace DataSpec::DNAANIM {
|
|
|
|
size_t ComputeBitstreamSize(size_t keyFrameCount, const std::vector<Channel>& channels) {
|
|
size_t bitsPerKeyFrame = 0;
|
|
for (const Channel& chan : channels) {
|
|
switch (chan.type) {
|
|
case Channel::Type::Rotation:
|
|
bitsPerKeyFrame += 1;
|
|
[[fallthrough]];
|
|
case Channel::Type::Translation:
|
|
case Channel::Type::Scale:
|
|
bitsPerKeyFrame += chan.q[0];
|
|
bitsPerKeyFrame += chan.q[1];
|
|
bitsPerKeyFrame += chan.q[2];
|
|
break;
|
|
case Channel::Type::KfHead:
|
|
bitsPerKeyFrame += 1;
|
|
break;
|
|
case Channel::Type::RotationMP3:
|
|
bitsPerKeyFrame += chan.q[0];
|
|
bitsPerKeyFrame += chan.q[1];
|
|
bitsPerKeyFrame += chan.q[2];
|
|
bitsPerKeyFrame += chan.q[3];
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
return (bitsPerKeyFrame * keyFrameCount + 31) / 32 * 4;
|
|
}
|
|
|
|
static QuantizedRot QuantizeRotation(const Value& quat, atUint32 div) {
|
|
float q = float(div) / (M_PIF / 2.0f);
|
|
zeus::simd_floats f(quat.simd);
|
|
assert(std::abs(f[1]) <= 1.f && "Out of range quat X component");
|
|
assert(std::abs(f[2]) <= 1.f && "Out of range quat Y component");
|
|
assert(std::abs(f[3]) <= 1.f && "Out of range quat Z component");
|
|
return {{
|
|
atInt32(std::asin(f[1]) * q),
|
|
atInt32(std::asin(f[2]) * q),
|
|
atInt32(std::asin(f[3]) * q),
|
|
},
|
|
(f[0] < 0.f)};
|
|
}
|
|
|
|
static Value DequantizeRotation(const QuantizedRot& v, atUint32 div) {
|
|
float q = (M_PIF / 2.0f) / float(div);
|
|
athena::simd_floats f = {
|
|
0.0f,
|
|
std::sin(v.v[0] * q),
|
|
std::sin(v.v[1] * q),
|
|
std::sin(v.v[2] * q),
|
|
};
|
|
f[0] = std::sqrt(std::max((1.0f - (f[1] * f[1] + f[2] * f[2] + f[3] * f[3])), 0.0f));
|
|
f[0] = v.w ? -f[0] : f[0];
|
|
Value retval;
|
|
retval.simd.copy_from(f);
|
|
return retval;
|
|
}
|
|
|
|
static Value DequantizeRotation_3(const QuantizedRot& v, atUint32 div) {
|
|
float q = 1.0f / float(div);
|
|
athena::simd_floats f = {
|
|
0.0f,
|
|
v.v[0] * q,
|
|
v.v[1] * q,
|
|
v.v[2] * q,
|
|
};
|
|
f[0] = std::sqrt(std::max((1.0f - (f[1] * f[1] + f[2] * f[2] + f[3] * f[3])), 0.0f));
|
|
f[0] = v.w ? -f[0] : f[0];
|
|
Value retval;
|
|
retval.simd.copy_from(f);
|
|
return retval;
|
|
}
|
|
|
|
bool BitstreamReader::dequantizeBit(const atUint8* data) {
|
|
atUint32 byteCur = (m_bitCur / 32) * 4;
|
|
atUint32 bitRem = m_bitCur % 32;
|
|
|
|
/* Fill 32 bit buffer with region containing bits */
|
|
/* Make them least significant */
|
|
atUint32 tempBuf = hecl::SBig(*reinterpret_cast<const atUint32*>(data + byteCur)) >> bitRem;
|
|
|
|
/* That's it */
|
|
m_bitCur += 1;
|
|
return tempBuf & 0x1;
|
|
}
|
|
|
|
atInt32 BitstreamReader::dequantize(const atUint8* data, atUint8 q) {
|
|
atUint32 byteCur = (m_bitCur / 32) * 4;
|
|
atUint32 bitRem = m_bitCur % 32;
|
|
|
|
/* Fill 32 bit buffer with region containing bits */
|
|
/* Make them least significant */
|
|
atUint32 tempBuf = hecl::SBig(*reinterpret_cast<const atUint32*>(data + byteCur)) >> bitRem;
|
|
|
|
/* If this shift underflows the value, buffer the next 32 bits */
|
|
/* And tack onto shifted buffer */
|
|
if ((bitRem + q) > 32) {
|
|
atUint32 tempBuf2 = hecl::SBig(*reinterpret_cast<const atUint32*>(data + byteCur + 4));
|
|
tempBuf |= (tempBuf2 << (32 - bitRem));
|
|
}
|
|
|
|
/* Mask it */
|
|
atUint32 mask = (1 << q) - 1;
|
|
tempBuf &= mask;
|
|
|
|
/* Sign extend */
|
|
atUint32 sign = (tempBuf >> (q - 1)) & 0x1;
|
|
if (sign)
|
|
tempBuf |= ~0u << q;
|
|
|
|
/* Return delta value */
|
|
m_bitCur += q;
|
|
return atInt32(tempBuf);
|
|
}
|
|
|
|
std::vector<std::vector<Value>> BitstreamReader::read(const atUint8* data, size_t keyFrameCount,
|
|
const std::vector<Channel>& channels, atUint32 rotDiv,
|
|
float transMult, float scaleMult) {
|
|
m_bitCur = 0;
|
|
std::vector<std::vector<Value>> chanKeys;
|
|
std::vector<QuantizedValue> chanAccum;
|
|
chanKeys.reserve(channels.size());
|
|
chanAccum.reserve(channels.size());
|
|
for (const Channel& chan : channels) {
|
|
chanAccum.push_back(chan.i);
|
|
|
|
chanKeys.emplace_back();
|
|
std::vector<Value>& keys = chanKeys.back();
|
|
keys.reserve(keyFrameCount);
|
|
switch (chan.type) {
|
|
case Channel::Type::Rotation: {
|
|
QuantizedRot qr = {{chan.i[0], chan.i[1], chan.i[2]}, false};
|
|
keys.emplace_back(DequantizeRotation(qr, rotDiv));
|
|
break;
|
|
}
|
|
case Channel::Type::Translation: {
|
|
keys.push_back({chan.i[0] * transMult, chan.i[1] * transMult, chan.i[2] * transMult});
|
|
break;
|
|
}
|
|
case Channel::Type::Scale: {
|
|
keys.push_back({chan.i[0] * scaleMult, chan.i[1] * scaleMult, chan.i[2] * scaleMult});
|
|
break;
|
|
}
|
|
case Channel::Type::KfHead: {
|
|
break;
|
|
}
|
|
case Channel::Type::RotationMP3: {
|
|
QuantizedRot qr = {{chan.i[1], chan.i[2], chan.i[3]}, bool(chan.i[0] & 0x1)};
|
|
keys.emplace_back(DequantizeRotation_3(qr, rotDiv));
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
for (size_t f = 0; f < keyFrameCount; ++f) {
|
|
#if DUMP_KEYS
|
|
fmt::print(stderr, fmt("\nFRAME {} {} {}\n"), f, (m_bitCur / 32) * 4, m_bitCur % 32);
|
|
int lastId = -1;
|
|
#endif
|
|
auto kit = chanKeys.begin();
|
|
auto ait = chanAccum.begin();
|
|
for (const Channel& chan : channels) {
|
|
#if DUMP_KEYS
|
|
if (chan.id != lastId) {
|
|
lastId = chan.id;
|
|
std::fputc('\n', stderr);
|
|
}
|
|
#endif
|
|
QuantizedValue& p = *ait;
|
|
switch (chan.type) {
|
|
case Channel::Type::Rotation: {
|
|
bool wBit = dequantizeBit(data);
|
|
p[0] += dequantize(data, chan.q[0]);
|
|
p[1] += dequantize(data, chan.q[1]);
|
|
p[2] += dequantize(data, chan.q[2]);
|
|
QuantizedRot qr = {{p[0], p[1], p[2]}, wBit};
|
|
kit->emplace_back(DequantizeRotation(qr, rotDiv));
|
|
#if DUMP_KEYS
|
|
fmt::print(stderr, fmt("{} R: {} {} {} {}\t"), chan.id, wBit, p[0], p[1], p[2]);
|
|
#endif
|
|
break;
|
|
}
|
|
case Channel::Type::Translation: {
|
|
atInt32 val1 = dequantize(data, chan.q[0]);
|
|
p[0] += val1;
|
|
atInt32 val2 = dequantize(data, chan.q[1]);
|
|
p[1] += val2;
|
|
atInt32 val3 = dequantize(data, chan.q[2]);
|
|
p[2] += val3;
|
|
kit->push_back({p[0] * transMult, p[1] * transMult, p[2] * transMult});
|
|
#if DUMP_KEYS
|
|
fmt::print(stderr, fmt("{} T: {} {} {}\t"), chan.id, p[0], p[1], p[2]);
|
|
#endif
|
|
break;
|
|
}
|
|
case Channel::Type::Scale: {
|
|
p[0] += dequantize(data, chan.q[0]);
|
|
p[1] += dequantize(data, chan.q[1]);
|
|
p[2] += dequantize(data, chan.q[2]);
|
|
kit->push_back({p[0] * scaleMult, p[1] * scaleMult, p[2] * scaleMult});
|
|
#if DUMP_KEYS
|
|
fmt::print(stderr, fmt("{} S: {} {} {}\t"), chan.id, p[0], p[1], p[2]);
|
|
#endif
|
|
break;
|
|
}
|
|
case Channel::Type::KfHead: {
|
|
dequantizeBit(data);
|
|
break;
|
|
}
|
|
case Channel::Type::RotationMP3: {
|
|
atInt32 val1 = dequantize(data, chan.q[0]);
|
|
p[0] += val1;
|
|
atInt32 val2 = dequantize(data, chan.q[1]);
|
|
p[1] += val2;
|
|
atInt32 val3 = dequantize(data, chan.q[2]);
|
|
p[2] += val3;
|
|
atInt32 val4 = dequantize(data, chan.q[3]);
|
|
p[3] += val4;
|
|
QuantizedRot qr = {{p[1], p[2], p[3]}, bool(p[0] & 0x1)};
|
|
kit->emplace_back(DequantizeRotation_3(qr, rotDiv));
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
++kit;
|
|
++ait;
|
|
}
|
|
#if DUMP_KEYS
|
|
std::fputc('\n', stderr);
|
|
#endif
|
|
}
|
|
|
|
return chanKeys;
|
|
}
|
|
|
|
void BitstreamWriter::quantizeBit(atUint8* data, bool val) {
|
|
atUint32 byteCur = (m_bitCur / 32) * 4;
|
|
atUint32 bitRem = m_bitCur % 32;
|
|
|
|
/* Fill 32 bit buffer with region containing bits */
|
|
/* Make them least significant */
|
|
*(atUint32*)(data + byteCur) = hecl::SBig(hecl::SBig(*(atUint32*)(data + byteCur)) | (val << bitRem));
|
|
|
|
m_bitCur += 1;
|
|
}
|
|
|
|
void BitstreamWriter::quantize(atUint8* data, atUint8 q, atInt32 val) {
|
|
atUint32 byteCur = (m_bitCur / 32) * 4;
|
|
atUint32 bitRem = m_bitCur % 32;
|
|
|
|
atUint32 masked = val & ((1 << q) - 1);
|
|
assert(((((val >> 31) & 0x1) == 0x1) || (((masked >> (q - 1)) & 0x1) == 0)) && "Twos compliment fail");
|
|
|
|
/* Fill 32 bit buffer with region containing bits */
|
|
/* Make them least significant */
|
|
*(atUint32*)(data + byteCur) = hecl::SBig(hecl::SBig(*(atUint32*)(data + byteCur)) | (masked << bitRem));
|
|
|
|
/* If this shift underflows the value, buffer the next 32 bits */
|
|
/* And tack onto shifted buffer */
|
|
if ((bitRem + q) > 32) {
|
|
*(atUint32*)(data + byteCur + 4) =
|
|
hecl::SBig(hecl::SBig(*(atUint32*)(data + byteCur + 4)) | (masked >> (32 - bitRem)));
|
|
}
|
|
|
|
m_bitCur += q;
|
|
}
|
|
|
|
std::unique_ptr<atUint8[]> BitstreamWriter::write(const std::vector<std::vector<Value>>& chanKeys, size_t keyFrameCount,
|
|
std::vector<Channel>& channels, atUint32 quantRange,
|
|
atUint32& rotDivOut, float& transMultOut, float& scaleMultOut,
|
|
size_t& sizeOut) {
|
|
m_bitCur = 0;
|
|
rotDivOut = quantRange; /* Normalized range of values */
|
|
float quantRangeF = float(quantRange);
|
|
|
|
/* Pre-pass to calculate translation multiplier */
|
|
float maxTransDelta = 0.0f;
|
|
float maxScaleDelta = 0.0f;
|
|
auto kit = chanKeys.begin();
|
|
for (Channel& chan : channels) {
|
|
switch (chan.type) {
|
|
case Channel::Type::Translation: {
|
|
zeus::simd<float> lastVal = {};
|
|
for (auto it = kit->begin(); it != kit->end(); ++it) {
|
|
const Value* key = &*it;
|
|
zeus::simd_floats f(key->simd - lastVal);
|
|
lastVal = key->simd;
|
|
maxTransDelta = std::max(maxTransDelta, std::fabs(f[0]));
|
|
maxTransDelta = std::max(maxTransDelta, std::fabs(f[1]));
|
|
maxTransDelta = std::max(maxTransDelta, std::fabs(f[2]));
|
|
}
|
|
break;
|
|
}
|
|
case Channel::Type::Scale: {
|
|
zeus::simd<float> lastVal = {};
|
|
for (auto it = kit->begin(); it != kit->end(); ++it) {
|
|
const Value* key = &*it;
|
|
zeus::simd_floats f(key->simd - lastVal);
|
|
lastVal = key->simd;
|
|
maxScaleDelta = std::max(maxScaleDelta, std::fabs(f[0]));
|
|
maxScaleDelta = std::max(maxScaleDelta, std::fabs(f[1]));
|
|
maxScaleDelta = std::max(maxScaleDelta, std::fabs(f[2]));
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
++kit;
|
|
}
|
|
transMultOut = maxTransDelta / quantRangeF + FLT_EPSILON;
|
|
scaleMultOut = maxScaleDelta / quantRangeF + FLT_EPSILON;
|
|
|
|
/* Output channel inits */
|
|
std::vector<QuantizedValue> initVals;
|
|
initVals.reserve(channels.size());
|
|
kit = chanKeys.begin();
|
|
for (Channel& chan : channels) {
|
|
chan.q[0] = 1;
|
|
chan.q[1] = 1;
|
|
chan.q[2] = 1;
|
|
switch (chan.type) {
|
|
case Channel::Type::Rotation: {
|
|
QuantizedRot qr = QuantizeRotation((*kit)[0], rotDivOut);
|
|
chan.i = qr.v;
|
|
initVals.push_back(chan.i);
|
|
break;
|
|
}
|
|
case Channel::Type::Translation: {
|
|
zeus::simd_floats f((*kit)[0].simd);
|
|
chan.i = {atInt32(f[0] / transMultOut), atInt32(f[1] / transMultOut), atInt32(f[2] / transMultOut)};
|
|
initVals.push_back(chan.i);
|
|
break;
|
|
}
|
|
case Channel::Type::Scale: {
|
|
zeus::simd_floats f((*kit)[0].simd);
|
|
chan.i = {atInt32(f[0] / scaleMultOut), atInt32(f[1] / scaleMultOut), atInt32(f[2] / scaleMultOut)};
|
|
initVals.push_back(chan.i);
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
++kit;
|
|
}
|
|
|
|
/* Pre-pass to analyze quantization factors for channels */
|
|
std::vector<QuantizedValue> lastVals = initVals;
|
|
kit = chanKeys.begin();
|
|
auto vit = lastVals.begin();
|
|
for (Channel& chan : channels) {
|
|
QuantizedValue& last = *vit++;
|
|
switch (chan.type) {
|
|
case Channel::Type::Rotation: {
|
|
for (auto it = kit->begin() + 1; it != kit->end(); ++it) {
|
|
QuantizedRot qrCur = QuantizeRotation(*it, rotDivOut);
|
|
chan.q[0] = std::max(chan.q[0], atUint8(qrCur.v.qFrom(last, 0)));
|
|
chan.q[1] = std::max(chan.q[1], atUint8(qrCur.v.qFrom(last, 1)));
|
|
chan.q[2] = std::max(chan.q[2], atUint8(qrCur.v.qFrom(last, 2)));
|
|
last = qrCur.v;
|
|
}
|
|
break;
|
|
}
|
|
case Channel::Type::Translation: {
|
|
for (auto it = kit->begin() + 1; it != kit->end(); ++it) {
|
|
zeus::simd_floats f(it->simd);
|
|
QuantizedValue cur = {atInt32(f[0] / transMultOut), atInt32(f[1] / transMultOut), atInt32(f[2] / transMultOut)};
|
|
chan.q[0] = std::max(chan.q[0], atUint8(cur.qFrom(last, 0)));
|
|
chan.q[1] = std::max(chan.q[1], atUint8(cur.qFrom(last, 1)));
|
|
chan.q[2] = std::max(chan.q[2], atUint8(cur.qFrom(last, 2)));
|
|
last = cur;
|
|
}
|
|
break;
|
|
}
|
|
case Channel::Type::Scale: {
|
|
for (auto it = kit->begin() + 1; it != kit->end(); ++it) {
|
|
zeus::simd_floats f(it->simd);
|
|
QuantizedValue cur = {atInt32(f[0] / scaleMultOut), atInt32(f[1] / scaleMultOut), atInt32(f[2] / scaleMultOut)};
|
|
chan.q[0] = std::max(chan.q[0], atUint8(cur.qFrom(last, 0)));
|
|
chan.q[1] = std::max(chan.q[1], atUint8(cur.qFrom(last, 1)));
|
|
chan.q[2] = std::max(chan.q[2], atUint8(cur.qFrom(last, 2)));
|
|
last = cur;
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
++kit;
|
|
}
|
|
|
|
/* Generate Bitstream */
|
|
sizeOut = ComputeBitstreamSize(keyFrameCount, channels);
|
|
std::unique_ptr<atUint8[]> newData(new atUint8[sizeOut]);
|
|
memset(newData.get(), 0, sizeOut);
|
|
|
|
lastVals = initVals;
|
|
for (size_t frame = 0; frame < keyFrameCount; ++frame) {
|
|
kit = chanKeys.begin();
|
|
vit = lastVals.begin();
|
|
for (const Channel& chan : channels) {
|
|
const Value& val = (*kit++)[frame + 1];
|
|
QuantizedValue& last = *vit++;
|
|
switch (chan.type) {
|
|
case Channel::Type::Rotation: {
|
|
QuantizedRot qrCur = QuantizeRotation(val, rotDivOut);
|
|
quantizeBit(newData.get(), qrCur.w);
|
|
quantize(newData.get(), chan.q[0], qrCur.v[0] - last.v[0]);
|
|
quantize(newData.get(), chan.q[1], qrCur.v[1] - last.v[1]);
|
|
quantize(newData.get(), chan.q[2], qrCur.v[2] - last.v[2]);
|
|
last = qrCur.v;
|
|
break;
|
|
}
|
|
case Channel::Type::Translation: {
|
|
zeus::simd_floats f(val.simd);
|
|
QuantizedValue cur = {atInt32(f[0] / transMultOut), atInt32(f[1] / transMultOut), atInt32(f[2] / transMultOut)};
|
|
quantize(newData.get(), chan.q[0], cur[0] - last[0]);
|
|
quantize(newData.get(), chan.q[1], cur[1] - last[1]);
|
|
quantize(newData.get(), chan.q[2], cur[2] - last[2]);
|
|
last = cur;
|
|
break;
|
|
}
|
|
case Channel::Type::Scale: {
|
|
zeus::simd_floats f(val.simd);
|
|
QuantizedValue cur = {atInt32(f[0] / scaleMultOut), atInt32(f[1] / scaleMultOut), atInt32(f[2] / scaleMultOut)};
|
|
quantize(newData.get(), chan.q[0], cur[0] - last[0]);
|
|
quantize(newData.get(), chan.q[1], cur[1] - last[1]);
|
|
quantize(newData.get(), chan.q[2], cur[2] - last[2]);
|
|
last = cur;
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
return newData;
|
|
}
|
|
|
|
} // namespace DataSpec::DNAANIM
|