/** Implementation based on the math in the book Watt, Policarpo. 3D Games: Real-time rendering and Software Technology, pp. 383-386. */
Quat Quat::Slerp(const Quat &q2, float t) const
{
assume(0.f <= t && t <= 1.f);
assume(IsNormalized());
assume(q2.IsNormalized());
float angle = this->Dot(q2);
float sign = 1.f; // Multiply by a sign of +/-1 to guarantee we rotate the shorter arc.
if (angle < 0.f)
{
angle = -angle;
sign = -1.f;
}
float a;
float b;
if (angle <= 0.97f) // perform spherical linear interpolation.
{
angle = acos(angle); // After this, angle is in the range pi/2 -> 0 as the original angle variable ranged from 0 -> 1.
float c = 1.f / sin(angle);
a = sin((1.f - t) * angle) * c;
b = sin(angle * t) * c;
}
else // If angle is close to taking the denominator to zero, resort to linear interpolation (and normalization).
{
a = 1.f - t;
b = t;
}
return (*this * (a * sign) + q2 * b).Normalized();
}
void ColorHistogram::ComputeMeanAndVariance() {
DCHECK(IsSparse()) << "Implemented for sparse histograms only.";
DCHECK(IsNormalized()) << "Implemented for normalized histograms only.";
mean_.fill(0);
var_.fill(0);
const ColorHistogramIndexLUT& lut = ColorHistogramIndexLUTFactory::Instance().GetLUT(
lum_bins_, color_bins_, color_bins_);
// Iteration uses simplification that all vals sum to one.
for (const auto& bin : sparse_bins_) {
const std::tuple<int, int, int>& idx_3d = lut.Ind2Sub(bin.first);
const float val = bin.second;
mean_[0] += std::get<0>(idx_3d) * val;
mean_[1] += std::get<1>(idx_3d) * val;
mean_[2] += std::get<2>(idx_3d) * val;
var_[0] += std::get<0>(idx_3d) * std::get<0>(idx_3d) * val;
var_[1] += std::get<1>(idx_3d) * std::get<1>(idx_3d) * val;
var_[2] += std::get<2>(idx_3d) * std::get<2>(idx_3d) * val;
}
var_[0] -= mean_[0] * mean_[0];
var_[1] -= mean_[1] * mean_[1];
var_[2] -= mean_[2] * mean_[2];
}
float ColorHistogram::KLDivergence(const ColorHistogram& rhs) const {
DCHECK(IsNormalized() && rhs.IsNormalized());
const double eps = 1e-10;
return 0.5 * GenericDistance(rhs, [eps](float a, float b) -> float {
const double ratio = (a + eps) / (b + eps);
return a * std::log(ratio) + b * std::log(1.0 / ratio);
});
}
float ColorHistogram::JSDivergence(const ColorHistogram& rhs) const {
DCHECK(IsNormalized() && rhs.IsNormalized());
const double eps = 1e-10;
return 0.5 * GenericDistance(rhs, [eps](float a, float b) -> float {
const double inv_mean = 1.0 / ((a + b) * 0.5 + eps);
const double ratio_a = (a + eps) * inv_mean;
const double ratio_b = (b + eps) * inv_mean;
return a * std::log(ratio_a) + b * std::log(ratio_b);
});
}
/// Compute the match between two WaveformAtAPointFT
/// NOTE: in python, the values for timeOffset, phaseOffset, and match are
/// also part of the return value (even though Match returns void), e.g.:
/// timeOffset, phaseOffset, match = Match(...)
void WaveformAtAPointFT::Match(const WaveformAtAPointFT& B,
const std::vector<double>& InversePSD,
double& timeOffset, double& phaseOffset,
double& match) const
{
/// \param[in] B WaveformAtAPointFT to compute match with
/// \param[in] InversePSD Spectrum used to weight contributions by frequencies to match
/// \param[out] timeOffset Time offset (in seconds) between the waveforms
/// \param[out] phaseOffset Phase offset used between the waveforms
/// \param[out] match Match between the two waveforms
const unsigned int n = NFreq(); // Only positive frequencies are stored in t
const unsigned int N = 2*(n-1); // But this is how many there really are
if(!IsNormalized() || !B.IsNormalized()) {
cerr << "\n\nWARNING!!! Matching non-normalized WaveformAtAPointFT objects. WARNING!!!\n" << endl;
}
if(n != B.NFreq() || n != InversePSD.size()) {
cerr << "Waveform sizes, " << n << " and " << B.NFreq()
<< ", are not compatible with InversePSD size, " << InversePSD.size() << "." << endl;
throw(GWFrames_VectorSizeMismatch);
}
const double eps = 1e-8;
const double df = F(1)-F(0);
const double df_B = B.F(1)-B.F(0);
const double rel_diff_df = std::fabs(1 - df/df_B);
if(rel_diff_df > eps) {
cerr << "Waveform frequency steps, " << df << " and " << df_B
<< ", are not compatible in Match: rel_diff="<< rel_diff_df << endl;
throw(GWFrames_VectorSizeMismatch);
}
// s1 s2* = (a1 + i b1) (a2 - i b2) = (a1 a2 + b1 b2) + i(b1 a2 - a1 b2)
WU::WrapVecDoub data(2*N);
for(unsigned int i=0; i<n; ++i) {
data.real(i) = (Re(i)*B.Re(i)+Im(i)*B.Im(i))*InversePSD[i];
data.imag(i) = (Im(i)*B.Re(i)-Re(i)*B.Im(i))*InversePSD[i];
}
idft(data);
unsigned int maxi=0;
double maxmag = std::sqrt(sqr(data.real(0)) + sqr(data.imag(0)));
for(unsigned int i=1; i<N; ++i) {
const double mag = std::sqrt(sqr(data.real(i)) + sqr(data.imag(i)));
if(mag>maxmag) { maxmag = mag; maxi = int(i); }
}
// note: assumes N is even and N >= maxi
timeOffset = (maxi<N/2 ? double(maxi)/(N*df) : -double(N-maxi)/(N*df));
phaseOffset = atan2(data.imag(maxi), data.real(maxi))/2.0;
/// The return from ifft is just the bare FFT sum, so we multiply by
/// df to get the continuum-analog FT. This is correct because the
/// input data (re,im) are the continuum-analog data, rather than
/// just the return from the bare FFT sum. See, e.g., Eq. (A.33)
/// [as opposed to Eq. (A.35)] of my (Mike Boyle's) thesis:
/// <http://thesis.library.caltech.edu/143>.
match = 4.0*df*maxmag;
return;
}
float4x4 MUST_USE_RESULT Quat::ToFloat4x4(const float4 &translation) const
{
assume(IsNormalized());
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
float4x4 m;
quat_to_mat4x4(q, translation.v, m.row);
return m;
#else
return float4x4(*this, translation.xyz());
#endif
}
float4x4 MUST_USE_RESULT Quat::ToFloat4x4() const
{
assume(IsNormalized());
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
float4x4 m;
quat_to_mat4x4(q, _mm_set_ps(1,0,0,0), m.row);
return m;
#else
return float4x4(*this);
#endif
}
float ColorHistogram::ChiSquareDist(const ColorHistogram& rhs) const {
DCHECK(IsNormalized() && rhs.IsNormalized());
return 0.5 * GenericDistance(rhs, [](float a, float b) -> float {
const float add = a + b;
if (fabs(add) > 1e-12) {
const float sub = a - b;
return sub * sub / add;
} else {
return 0.0f;
}
});
}
/** Implementation based on the math in the book Watt, Policarpo. 3D Games: Real-time rendering and Software Technology, pp. 383-386. */
Quat MUST_USE_RESULT Quat::Slerp(const Quat &q2, float t) const
{
///\todo SSE.
assume(0.f <= t && t <= 1.f);
assume(IsNormalized());
assume(q2.IsNormalized());
float angle = this->Dot(q2);
float sign = 1.f; // Multiply by a sign of +/-1 to guarantee we rotate the shorter arc.
if (angle < 0.f)
{
angle = -angle;
sign = -1.f;
}
float a;
float b;
if (angle <= 0.97f) // perform spherical linear interpolation.
{
angle = Acos(angle); // After this, angle is in the range pi/2 -> 0 as the original angle variable ranged from 0 -> 1.
float angleT = t*angle;
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
// Compute three sines in one go with SSE.
simd4f s = set_ps(0.f, angleT, angle - angleT, angle);
s = sin_ps(s);
simd4f denom = shuffle1_ps(s, _MM_SHUFFLE(0, 0, 0, 0));
s = div_ps(s, denom);
a = s4f_y(s);
b = s4f_z(s);
#else
float s[3] = { Sin(angle), Sin(angle - angleT), Sin(angleT) };
float c = 1.f / s[0];
a = s[1] * c;
b = s[2] * c;
#endif
}
else // If angle is close to taking the denominator to zero, resort to linear interpolation (and normalization).
{
a = 1.f - t;
b = t;
}
return (*this * (a * sign) + q2 * b).Normalized();
}
void ColorHistogram::NormalizeToOne() {
if (IsNormalized()) {
DLOG(WARNING) << "Normalization of normalized histogram requested. Ignored.";
}
is_normalized_ = true;
if (weight_sum_ == 0) {
return;
}
const float denom = 1.0f / weight_sum_;
if (!IsSparse()) {
for (auto& bin : bins_) {
bin *= denom;
}
} else {
for (auto& bin : sparse_bins_) {
bin.second *= denom;
}
}
}
Quat Quat::Inverted() const
{
assume(IsNormalized());
assume(IsInvertible());
return Conjugated();
}
void Quat::Inverse()
{
assume(IsNormalized());
assume(IsInvertible());
Conjugate();
}
Quat MUST_USE_RESULT Quat::Inverted() const
{
assume(IsNormalized());
assume(IsInvertible());
return Conjugated();
}
void ALaser::checkLaserCollisions(float dt)
{
FHitResult hit;
FCollisionQueryParams queryParam;
queryParam.bFindInitialOverlaps = false;
queryParam.bReturnFaceIndex = true;
FCollisionObjectQueryParams objectParam = objectParam.DefaultObjectQueryParam;
if ( !canHitBall ) queryParam.AddIgnoredActor( GetWorld()->GetGameState<AProjectTapGameState>()->GetPlayer() );
auto pos = GetActorLocation();
auto rayStart = pos + dir * 5.0f;
auto laserVector = dir * length;
auto laserEmitter = laserParticle->EmitterInstances[0];
//ray cast to see if laser hits anything
GetWorld()->LineTraceSingleByObjectType(hit,rayStart, pos + laserVector, objectParam,queryParam);
auto hitActor = hit.Actor.Get();
if (hitActor != nullptr)
{
currHitPoint = hit.ImpactPoint;
if(!laserSparkParticle->bIsActive) laserSparkParticle->ActivateSystem();
//kills ball if laser hits it
auto ball = Cast<ABallPawn>(hitActor);
if (ball != nullptr)
{
ball->Kill();
laserEmitter->SetBeamTargetPoint(hit.ImpactPoint, 0);
KillSubLaser();
}
else
{
//if not set laser end point
laserEmitter->SetBeamTargetPoint(hit.ImpactPoint, 0);
bool typeFound = false;
//if hits deflective tile then spawn a new laser object
auto tile = Cast<ADeflectiveTile>(hitActor);
bool isFrame = false;
if (tile != nullptr)
{
typeFound = true;
TArray<USceneComponent*> Children;
tile->frameCollisionsComponent->GetChildrenComponents(true, Children);
for(int i = 0; i < Children.Num() && typeFound ; ++i)
{
// printonscreen(hit.Component.Get()->GetName());
// printonscreen(Children[i]->GetName());
if(hit.Component.Get() == Children[i])
{
typeFound = false;
isFrame = true;
}
}
//cut the laser length to make sure new sub laser start doesn't hit the same object
if (typeFound && CanSpawnSubLaser()) SpawnSubLaser(hit.ImpactPoint + hit.ImpactNormal * 10.0f, hit.ImpactNormal);
}
//if sub laser already exists then keep updating its rotation and position
auto subLaserExistsHitDeflectiveTile = currentDepth < MAX_DEPTH && nextLaser != nullptr && tile != nullptr;
if (subLaserExistsHitDeflectiveTile)
{
auto incomingVector = hit.ImpactPoint - GetActorLocation();
//if the incoming vector's angle is too small then kill sublasers to avoid laser flickering
auto dot = FVector::DotProduct(-incomingVector.GetSafeNormal(), hit.ImpactNormal);
auto angle = FMath::RadiansToDegrees(FMath::Acos(dot));
if (angle < 70.0f)
{
auto newDir = FMath::GetReflectionVector(incomingVector, hit.ImpactNormal);
auto start = hit.ImpactPoint + newDir * 2.0f;
nextLaser->SetActorLocation(hit.ImpactPoint);
nextLaser->dir = newDir.IsNormalized() ? newDir : newDir.GetSafeNormal();
nextLaser->laserParticle->EmitterInstances[0]->SetBeamSourcePoint(hit.ImpactPoint, 0);
nextLaser->laserParticle->EmitterInstances[0]->SetBeamTargetPoint(start + newDir * length, 0);
}
else
{
KillSubLaser();
}
}
//if the laser hits turret then kills it
if (!typeFound)
{
auto turret = Cast<ATurretPawn>(hitActor);
if (turret != nullptr)
{
typeFound = true;
turret->Damage(2.0f);
}
}
APortalTile* portal = nullptr;
if (!typeFound)
{
portal = Cast<APortalTile>(hitActor);
if (CanSpawnSubLaser() && portal != nullptr)
{
typeFound = true;
auto relativePos = hit.ImpactPoint - hit.GetComponent()->GetComponentLocation();
currHitPoint = relativePos + portal->GetActorLocation();
SpawnSubLaser(currHitPoint, hit.ImpactNormal);
}
}
auto subLaserExistsHitPortalTile = currentDepth < MAX_DEPTH && nextLaser != nullptr && portal != nullptr;
if (subLaserExistsHitPortalTile)
{
FVector newSourcePos;
portal->GetLaserPortalTransportedLocation(hit.GetComponent(), nextLaser->dir, newSourcePos);
nextLaser->SetActorLocation(newSourcePos);
nextLaser->laserParticle->EmitterInstances[0]->SetBeamSourcePoint(newSourcePos, 0);
nextLaser->laserParticle->EmitterInstances[0]->SetBeamTargetPoint(newSourcePos + nextLaser->dir * length, 0);
}
bool notHitDeflectiveTile = tile == nullptr || isFrame;
bool notHitPortal = portal == nullptr;
if (notHitDeflectiveTile && notHitPortal)
{
KillSubLaser();
}
}
laserSparkParticle->SetWorldLocation(currHitPoint);
laserSparkParticle->SetWorldRotation(FVector(currHitPoint -
GetActorLocation()).GetSafeNormal().Rotation().Quaternion());
}
else
{
currHitPoint = laserVector;
laserEmitter->SetBeamTargetPoint(pos + currHitPoint, 0);
laserSparkParticle->SetWorldLocation( pos + currHitPoint );
laserSparkParticle->SetWorldRotation( FVector( (pos + currHitPoint) -
GetActorLocation() ).GetSafeNormal().Rotation().Quaternion() );
KillSubLaser();
}
//only root laser can have an emitter mesh
if (currentDepth != 0)
{
//update laser emitter rotation
mesh->SetVisibility(false);
}
else
{
mesh->SetWorldRotation(dir.Rotation());
}
}
void ColorHistogram::MergeWithHistogram(const ColorHistogram& rhs) {
DCHECK(is_sparse_ == rhs.is_sparse_) << "Sparsity differs.";
DCHECK(is_normalized_ == rhs.is_normalized_) << "Normalization differs.";
const double n = weight_sum_ + rhs.weight_sum_;
if (n == 0) {
return;
}
// Weighted merge for normalized histograms.
const float n_l = weight_sum_ / n;
const float n_r = rhs.weight_sum_ / n;
// New weight_sum equals sum of both.
weight_sum_ = n;
double weighted_bin_sum = 0;
if (!IsSparse()) {
if (IsNormalized()) {
for (int i = 0; i < total_bins_; ++i) {
bins_[i] = bins_[i] * n_l + rhs.bins_[i] * n_r;
weighted_bin_sum += bins_[i];
}
// Re-Normalize.
const float denom = 1.0f / weighted_bin_sum;
for (float& bin : bins_) {
bin *= denom;
}
} else {
for (int i = 0; i < total_bins_; ++i) {
bins_[i] += rhs.bins_[i];
}
}
} else {
// Sparse version.
if (IsNormalized()) {
for (auto& bin : sparse_bins_) {
const auto rhs_bin_iter = rhs.sparse_bins_.find(bin.first);
if (rhs_bin_iter != rhs.sparse_bins_.end()) {
bin.second = bin.second * n_l + rhs_bin_iter->second * n_r;
} else {
bin.second *= n_l;
}
weighted_bin_sum += bin.second;
}
// Process rhs bins that we might have missed.
for (const auto& rhs_bin : rhs.sparse_bins_) {
const auto bin_iter = sparse_bins_.find(rhs_bin.first);
if (bin_iter == sparse_bins_.end()) {
weighted_bin_sum += (
(sparse_bins_[rhs_bin.first] = rhs_bin.second * n_r));
}
}
// Normalize.
const float denom = 1.0f / weighted_bin_sum;
for (auto& bin : sparse_bins_) {
bin.second *= denom;
}
} else {
for (auto& bin : sparse_bins_) {
const auto rhs_bin_iter = rhs.sparse_bins_.find(bin.first);
if (rhs_bin_iter != rhs.sparse_bins_.end()) {
bin.second += rhs_bin_iter->second;
}
}
// Process rhs bins that we might have missed.
for (const auto& rhs_bin : rhs.sparse_bins_) {
const auto bin_iter = sparse_bins_.find(rhs_bin.first);
if (bin_iter == sparse_bins_.end()) {
sparse_bins_.insert(rhs_bin);
}
}
}
}
}
Quat MUST_USE_RESULT Quat::Slerp(const Quat &q2, float t) const
{
assume(0.f <= t && t <= 1.f);
assume(IsNormalized());
assume(q2.IsNormalized());
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
simd4f angle = dot4_ps(q, q2.q); // <q, q2.q>
simd4f neg = cmplt_ps(angle, zero_ps()); // angle < 0?
neg = and_ps(neg, set1_ps_hex(0x80000000)); // Convert 0/0xFFFFFFFF mask to a 0x/0x80000000 mask.
// neg = s4i_to_s4f(_mm_slli_epi32(s4f_to_s4i(neg), 31)); // A SSE2-esque way to achieve the above would be this, but this seems to clock slower (12.04 clocks vs 11.97 clocks)
angle = xor_ps(angle, neg); // if angle was negative, make it positive.
simd4f one = set1_ps(1.f);
angle = min_ps(angle, one); // If user passed t > 1 or t < -1, clamp the range.
// Compute a fast polynomial approximation to arccos(angle).
// arccos(x): (-0.69813170079773212f * x * x - 0.87266462599716477f) * x + 1.5707963267948966f;
angle = madd_ps(msub_ps(mul_ps(set1_ps(-0.69813170079773212f), angle), angle, set1_ps(0.87266462599716477f)), angle, set1_ps(1.5707963267948966f));
// Shuffle an appropriate vector from 't' and 'angle' for computing two sines in one go.
simd4f T = _mm_set_ss(t); // (.., t)
simd4f oneSubT = sub_ps(one, T); // (.., 1-t)
T = _mm_movelh_ps(T, oneSubT); // (.., 1-t, .., t)
angle = mul_ps(angle, T); // (.., (1-t)*angle, .., t*angle)
// Compute a fast polynomial approximation to sin(t*angle) and sin((1-t)*angle).
// Here could use "angle = sin_ps(angle);" for precision, but favor speed instead with the following polynomial expansion:
// sin(x): ((5.64311797634681035370e-03 * x * x - 1.55271410633428644799e-01) * x * x + 9.87862135574673806965e-01) * x
simd4f angle2 = mul_ps(angle, angle);
angle = mul_ps(angle, madd_ps(madd_ps(angle2, set1_ps(5.64311797634681035370e-03f), set1_ps(-1.55271410633428644799e-01f)), angle2, set1_ps(9.87862135574673806965e-01f)));
// Compute the final lerp factors a and b to scale q and q2.
simd4f a = zzzz_ps(angle);
simd4f b = xxxx_ps(angle);
a = xor_ps(a, neg);
a = mul_ps(q, a);
a = madd_ps(q2, b, a);
// The lerp above generates an unnormalized quaternion which needs to be renormalized.
return mul_ps(a, rsqrt_ps(dot4_ps(a, a)));
#else
float angle = this->Dot(q2);
float sign = 1.f; // Multiply by a sign of +/-1 to guarantee we rotate the shorter arc.
if (angle < 0.f)
{
angle = -angle;
sign = -1.f;
}
float a;
float b;
if (angle < 0.999) // perform spherical linear interpolation.
{
// angle = Acos(angle); // After this, angle is in the range pi/2 -> 0 as the original angle variable ranged from 0 -> 1.
angle = (-0.69813170079773212f * angle * angle - 0.87266462599716477f) * angle + 1.5707963267948966f;
float ta = t*angle;
#ifdef MATH_USE_SINCOS_LOOKUPTABLE
// If Sin() is based on a lookup table, prefer that over polynomial approximation.
a = Sin(angle - ta);
b = Sin(ta);
#else
// Not using a lookup table, manually compute the two sines by using a very rough approximation.
float ta2 = ta*ta;
b = ((5.64311797634681035370e-03f * ta2 - 1.55271410633428644799e-01f) * ta2 + 9.87862135574673806965e-01f) * ta;
a = angle - ta;
float a2 = a*a;
a = ((5.64311797634681035370e-03f * a2 - 1.55271410633428644799e-01f) * a2 + 9.87862135574673806965e-01f) * a;
#endif
}
else // If angle is close to taking the denominator to zero, resort to linear interpolation (and normalization).
{
a = 1.f - t;
b = t;
}
// Lerp and renormalize.
return (*this * (a * sign) + q2 * b).Normalized();
#endif
}
