Quaternion& Quaternion::normalize()
{
// The zero-quaternion stays the zero-quaternion.
if(nearZero(this->x()) &&
nearZero(this->y()) &&
nearZero(this->z()) &&
nearZero(this->w()) ) {
return this->x(0.0f).y(0.0f).z(0.0f).w(0.0f);
}
float l = this->len();
// Very little quaternion will be stretched to a unit quaternion in one direction.
if(nearZero(l)) {
if((this->x() >= this->y())
&& (this->x() >= this->z())
&& (this->x() >= this->w())
&& (this->x() >= 0.0f)) {
return this->x(1.0f).y(0.0f).z(0.0f).w(0.0f);
} else if((this->x() <= this->y())
&& (this->x() <= this->z())
&& (this->x() <= this->w())
&& (this->x() <= 0.0f)) {
return this->x(-1.0f).y(0.0f).z(0.0f).w(0.0f);
} else {
if(this->y() >= this->z()
&& this->y() >= this->w()
&& this->y() >= 0.0f) {
return this->x(0.0f).y(1.0f).z(0.0f).w(0.0f);
} else if(this->y() <= this->z()
&& this->y() <= this->w()
&& this->y() <= 0.0f) {
return this->x(0.0f).y(-1.0f).z(0.0f).w(0.0f);
} else {
if(this->z() >= this->w()
&& this->z() >= 0.0f) {
return this->x(0.0f).y(0.0f).z(1.0f).w(0.0f);
} else if(this->z() <= this->w()
&& this->z() <= 0.0f) {
return this->x(0.0f).y(0.0f).z(-1.0f).w(0.0f);
} else {
return this->x(0.0f).y(0.0f).z(0.0f).w(this->w() >= 0.0f ? 1.0f : -1.0f);
}
}
}
} else {
// Follows the usual normalization rule.
float m = 1.0f / l;
return this->x(this->x()*m).y(this->y()*m).z(this->z()*m).w(this->w()*m);
}
}
LineSegmentPtr StatusStructure::fragmentLeftOf(const Point& point) const {
float dist;
size_t closestIndex = getClosestTo(point, dist);
auto frag = getAt(closestIndex);
if (frag == nullptr) {
return nullptr;
}
if (dist > 0 || nearZero(dist)) {
// The fragment at closestIndex is not left of the point (equal or right)
// find the first left neighbour that is left of the point.
while (dist > 0 || nearZero(dist)) {
size_t leftNeighbourIndex = getLeftNeighbourIndex(closestIndex);
if (leftNeighbourIndex == closestIndex) {
// no left neighbour...
return nullptr;
}
closestIndex = leftNeighbourIndex;
frag = getAt(closestIndex);
if (frag == nullptr) {
return nullptr;
}
dist = getPositionOnScanLine(frag, point);
}
} else {
// There could be other fragments that intersect the scan line at the same position as frag (e.g same startpoint as frag or intersection with frag)
// Move right until we find a fragment that is right (or equal) to the point.. return the last fragment that was left
for (;;) {
size_t rightNeighbourIndex = getRightNeighbourIndex(closestIndex);
auto rightFrag = getAt(rightNeighbourIndex);
dist = getPositionOnScanLine(rightFrag, point);
if (dist > 0 || nearZero(dist) || rightFrag == nullptr || rightNeighbourIndex == closestIndex) {
break;
}
closestIndex = rightNeighbourIndex;
frag = rightFrag;
}
}
return frag;
}
double
Black::impliedVol( double strike, // option strike
double forwardPrice, // underlying asset's forward value
double marketPrice, // market price of option
double rate, // risk free rate of interest
double T ) const // time to maturity (year fraction)
{
int iterations = 100;
double vol = 0.5;
double sqrtT = sqrt(T);
double logTerm = log(forwardPrice / strike);
while (--iterations)
{
double p1 = value(strike, forwardPrice, vol, rate, T, true) - marketPrice;
if (nearZero(p1))
break;
// vega(vol)
double d1 = ( logTerm + (((vol * vol) / 2.0)) * T ) / (vol * sqrtT);
double p2 = forwardPrice * exp( -rate * T ) * sqrtT * DN(d1);
vol = vol - (p1 / p2);
}
return vol;
}
LineSegmentPtr StatusStructure::fragmentRightOf(const Point& point) const {
float dist;
size_t closestIndex = getClosestTo(point, dist);
auto frag = getAt(closestIndex);
if (frag == nullptr) {
return nullptr;
}
if (dist < 0 || nearZero(dist)) {
// find the first right neighbour that is right of the point
while (dist < 0 || nearZero(dist)) {
size_t rightNeighbourIndex = getRightNeighbourIndex(closestIndex);
if (rightNeighbourIndex == closestIndex) {
return nullptr;
}
closestIndex = rightNeighbourIndex;
frag = getAt(closestIndex);
if (frag == nullptr) {
return nullptr;
}
dist = getPositionOnScanLine(frag, point);
}
} else if (nearZero(dist)) {
// find the last left neighbour that is still right of the point
for (;;) {
size_t leftNeighbourIndex = getLeftNeighbourIndex(closestIndex);
auto leftFrag = getAt(leftNeighbourIndex);
dist = getPositionOnScanLine(leftFrag, point);
if (dist < 0 || nearZero(dist) || leftFrag == nullptr || leftNeighbourIndex == closestIndex) {
break;
}
closestIndex = leftNeighbourIndex;
frag = leftFrag;
}
}
return frag;
}
AnimationPtr ModelInstance::playOneShot(const std::string anim, float speed, float fadeInTime, float fadeOutTime, TimeFunction* control)
{
CoreAnimationPtrC coreAnim = this->findAnimToUse(anim);
if(nearZero(coreAnim->duration())) {
return m_mixer->oneshot(AnimationPtr(new Animation(coreAnim, speed)));
}
// Note: same as in playCycle.
TimeFunction* pWeightFun = new FadeOutTF(fadeOutTime/coreAnim->duration(), new FadeInTF(fadeInTime/coreAnim->duration(), new ConstantTF(1.0f)));
return m_mixer->oneshot(AnimationPtr(new Animation(m_coremdl->animation(anim), speed, control, pWeightFun)));
}
AnimationPtr ModelInstance::playCycle(const std::string anim, float speed, float fadeInTime, float weight, TimeFunction* control)
{
CoreAnimationPtrC coreAnim = this->findAnimToUse(anim);
if(nearZero(coreAnim->duration())) {
return m_mixer->play(AnimationPtr(new Animation(coreAnim, speed)));
}
// Note: as the FadeInTF uses normalized-time, we need to divide our
// absolue time by the animation's duration in order to normalize it.
TimeFunction* pWeightFun = new FadeInTF(fadeInTime/coreAnim->duration(), new ConstantTF(weight));
return m_mixer->play(AnimationPtr(new Animation(coreAnim, speed, control, pWeightFun)));
}
Quaternion Quaternion::slerp(const Quaternion& q2, float between) const
{
float cosTheta = this->dot(q2);
cosTheta = std::min(cosTheta, 1.0f);
cosTheta = std::max(cosTheta, -1.0f); // Clamp to [-1, 1] for the acos.
float theta = acos(cosTheta);
float sinTheta = sin(theta);
float w1, w2;
if(nearZero(sinTheta)) {
// Quaternions a and b are nearly the same, do linear interpolation.
w1 = 1.0f - between;
w2 = between;
} else {
w1 = float(sin((1.0f-between)*theta) / sinTheta);
w2 = float(sin(between*theta) / sinTheta);
}
return ((*this)*w1 + q2*w2).normalize();
}
///////////////////////////////////////
// Quaternion comparison operations. //
///////////////////////////////////////
bool Quaternion::operator ==(const Quaternion &in_q) const
{
Quaternion diff = *this - in_q;
return nearZero(diff.x()) && nearZero(diff.y()) && nearZero(diff.z());
}
