//
// Copyright 2016 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//
////////////////////////////////////////////////////////////////////////
// This file is generated by a script.  Do not edit directly.  Edit the
// quat.template.cpp file to make changes.

#include "pxr/pxr.h"
#include "pxr/base/gf/quat{{ SUFFIX }}.h"
#include "pxr/base/gf/ostreamHelpers.h"
#include "pxr/base/gf/vec4{{ SUFFIX }}.h"
#include "pxr/base/tf/type.h"

{% for S in SCALARS if S != SCL -%}
#include "pxr/base/gf/quat{{ SCALAR_SUFFIX(S) }}.h"
{% endfor %}

PXR_NAMESPACE_OPEN_SCOPE

TF_REGISTRY_FUNCTION(TfType) {
    TfType::Define<{{ QUAT }}>();
}

{% for S in SCALARS if S != SCL %}
{{ QUAT }}::{{ QUAT }}({{ QUATNAME(S) }} const &other)
    : _imaginary(other.GetImaginary())
    , _real(other.GetReal())
{
}
{% endfor %}

std::ostream& 
operator<<(std::ostream &out, {{ QUAT }} const &q)
{
    GfVec3{{ SUFFIX }} i = q.GetImaginary();
    {{ SCL }} real = q.GetReal();
    return out << GfVec4{{ SUFFIX }}(real, i[0], i[1], i[2]);
}

{{ SCL }}
{{ QUAT }}::Normalize({{ SCL }} eps)
{
    {{ SCL }} length = GetLength();

    if (length < eps)
        *this = GetIdentity();
    else
        *this /= length;

    return length;
}

{{ QUAT }} &
{{ QUAT }}::operator *=(const {{ QUAT }} &q)
{
    {{ SCL }} r1 = GetReal();
    {{ SCL }} r2 = q.GetReal();
    const GfVec3{{ SUFFIX }} &i1 = GetImaginary();
    const GfVec3{{ SUFFIX }} &i2 = q.GetImaginary();

    {{ SCL }} r = r1 * r2 - GfDot(i1, i2);

    GfVec3{{ SUFFIX }} i(
        r1 * i2[0] + r2 * i1[0] + (i1[1] * i2[2] - i1[2] * i2[1]),
        r1 * i2[1] + r2 * i1[1] + (i1[2] * i2[0] - i1[0] * i2[2]),
        r1 * i2[2] + r2 * i1[2] + (i1[0] * i2[1] - i1[1] * i2[0]));

    SetReal(r);
    SetImaginary(i);

    return *this;
}

{{ QUAT }}
GfSlerp(const {{ QUAT }}& q0, const {{ QUAT }}& q1, double alpha)
{
    return GfSlerp(alpha, q0, q1);
}

{{ QUAT }}
GfSlerp(double alpha, const {{ QUAT }}& q0, const {{ QUAT }}& q1)
{
    double cosTheta = q0.GetImaginary() * q1.GetImaginary() +
        q0.GetReal() * q1.GetReal();
    bool flip1 = false;

    if (cosTheta < 0.0) {
        cosTheta = -cosTheta;
        flip1 = true;
    }

    double scale0, scale1;

    if (1.0 - cosTheta > 0.00001 ) {
        // standard case
        {{ SCL }} theta = acos(cosTheta),
               sinTheta = sin(theta);

        scale0 = sin((1.0 - alpha) * theta) / sinTheta;
        scale1 = sin(alpha * theta) / sinTheta;
    } else {        
        // rot0 and rot1 very close - just do linear interp and renormalize.
        scale0 = 1.0 - alpha;
        scale1 = alpha;
    }

    if (flip1)
        scale1 = -scale1;

    return scale0 * q0 + scale1 * q1;
}

PXR_NAMESPACE_CLOSE_SCOPE
