HSVColor::HSVColor(const Color& c)
{
const RGBColor* rgbc = NULL;
const HSVColor* hsvc = NULL;
const HSVConeColor* hsvcc = NULL;
if ( (rgbc = dynamic_cast<const RGBColor*>(&c)) != NULL)
{
appear_t rgbMin, rgbMax, delta;
appear_t r = rgbc->R(), g = rgbc->G(), b = rgbc->B();
rgbMin = std::min(r,std::min(g,b));
rgbMax = std::max(r,std::max(g,b));
delta = rgbMax - rgbMin;
V() = rgbMax;
S() = 0;
if (rgbMax > 0)
S() = delta / rgbMax;
// fixme: handle floating point check here.
NUKLEI_ASSERT( 0 <= S() && S() <= 1 );
H() = 0;
if (delta > 0)
{
if (rgbMax == r && rgbMax != g)
H() += (g - b) / delta;
if (rgbMax == g && rgbMax != b)
H() += (2 + (b - r) / delta);
if (rgbMax == b && rgbMax != r)
H() += (4 + (r - g) / delta);
H() *= 60;
if (H() < 0)
H() += 360;
}
H() *= M_PI / 180;
}
else if ( (hsvc = dynamic_cast<const HSVColor*>(&c)) != NULL) c_ = hsvc->c_;
else if ( (hsvcc = dynamic_cast<const HSVConeColor*>(&c)) != NULL)
{
NUKLEI_ASSERT(hsvcc->W() > 0);
V() = hsvcc->WV() / hsvcc->W();
if (V() > 0)
{
S() = std::sqrt( std::pow(hsvcc->SCosH(), 2) + std::pow(hsvcc->SSinH(), 2) ) / V();
if (hsvcc->SSinH() >= 0) H() = ACos(hsvcc->SCosH() / (V()*S()));
else H() = 2*M_PI-ACos(hsvcc->SCosH() / (V()*S()));
}
else if (V() == 0)
{
S() = 1;
H() = 0;
}
else NUKLEI_ASSERT(false);
}
else NUKLEI_THROW("Unknown color type");
assertConsistency();
}
// Spherical linear interpolation of two quaternions p and q, with parameter t, result in slerp
void Slerp
(
const CQuaternion& p,
const CQuaternion& q,
const TFloat32 t,
CQuaternion& slerp
)
{
// Slerp formula: qt = (sin((1-t)*theta)*p + sin(t*theta)*q) / sin theta , theta is angle
// between quaternions. First get cos of angle between quaternions - can use dot product if
// we assume our quaternions are normalised
TFloat32 cosTheta = Dot( p, q );
// Two routes round a circle from q0 to q1, choose the short one with this test
if (cosTheta >= 0.0f)
{
// Slerp formula prone to error with small angles, ensure that is not the case
if (!AreEqual( cosTheta, 1.0f ))
{
// Slerp calculation
TFloat32 theta = ACos( cosTheta );
// Now we have p, q, t and theta. Calculate slerp from the equation in the notes
TFloat32 invSinTheta = 1.0f / Sin( theta );
TFloat32 w1 = Sin( (1.0f-t)*theta ) * invSinTheta;
TFloat32 w2 = Sin( t*theta ) * invSinTheta;
slerp = p*w1 + q*w2;
}
else
{
// Small angle - lerp calculation is better
slerp = p*(1.0f-t) + q*t;
}
}
else
{
// Want opposite route round circle - negate first quaternion, otherwise same formula
if (!AreEqual( cosTheta, -1.0f ))
{
TFloat32 theta = ACos( -cosTheta);
// Same calculation as above but use (t-1) instead of (1-t), to perform negation
TFloat32 invSinTheta = 1.0f / Sin( theta );
TFloat32 w1 = Sin( (t-1.0f)*theta ) * invSinTheta;
TFloat32 w2 = Sin( t*theta ) * invSinTheta;
slerp = p*w1 + q*w2;
}
else
{
// Small angle - lerp calculation is better, but use (t-1) instead of (1-t)
slerp = p*(t-1.0f) + q*t;
}
}
}
bool Quat::Equals(const Quat& rhs, const Real toleranceRadian) const
{
Real fCos = Dot(rhs);
Real angle = ACos(fCos);
return (abs(angle) <= toleranceRadian) || IsEqual(angle, PI, toleranceRadian);
}
Quaternion Quaternion::Ln() const
{
// If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit mag, then
// log(q) = A*(x*i+y*j+z*k). If sin(A) is near zero, use log(q) =
// sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.
Quaternion result;
result.w = 0.0;
if (Abs(w) < 1.0)
{
float angle = ACos(w);
float sinA = Sin(angle);
if (Abs(sinA) >= EPSILON)
{
float fCoeff = angle / sinA;
result.x = fCoeff * x;
result.y = fCoeff * y;
result.z = fCoeff * z;
return result;
}
}
result.x = x;
result.y = y;
result.z = z;
return result;
}
Quaternion Quaternion::Slerp(
float t,
const Quaternion &q1,
const Quaternion &q2,
bool shortestPath)
{
float cosA = Dot(q1, q2);
float angle = ACos(cosA);
if (Abs(angle) < EPSILON)
{
return q1;
}
float sinA = Sin(angle);
float invSinA = 1.0f / sinA;
float coeff0 = Sin((1.0f - t) * angle) * invSinA;
float coeff1 = Sin(t * angle) * invSinA;
// Do we need to invert rotation?
if (cosA < 0.0f && shortestPath)
{
coeff0 = -coeff0;
// taking the complement requires renormalisation
Quaternion result(coeff0 * q1 + coeff1 * q2);
result.Normalise();
return result;
}
else
{
return coeff0 * q1 + coeff1 * q2;
}
}
Quat Quat::SLERP( float t, const Quat& q ) const
{
if( *this == q )
{
return *this;
}
Quat ThisCopy = *this;
// Calculate angle between quaternions
float costheta = Clamp( Dot( q ), -1.0f, 1.0f );
if( costheta < 0.f )
{
costheta = -costheta;
ThisCopy.Negate();
}
float theta = ACos( costheta );
float sinTheta = Sin( theta );
float ratioA, ratioB;
if( sinTheta < EPSILON )
{
ratioA = 0.5f;
ratioB = 0.5f;
}
else
{
float InvSinTheta = 1.0f / sinTheta;
ratioA = Sin( ( 1.0f - t ) * theta ) * InvSinTheta;
ratioB = Sin( t * theta ) * InvSinTheta;
}
// Calculate interpolated quaternion from the ratios
return ( ( ThisCopy * ratioA ) + ( q * ratioB ) );
}
/* decides if we can kick staright to the decired point around the player
without a collision.
(EndDist, dir) is a relative polar vector for the ball's final position
closeMarg is what the radius of the player is considered to be */
int is_straight_kick(float dir, float EndDist, float closeMarg)
{
Vector btraj = Polar2Vector(EndDist, dir) - Mem->BallRelativeToBodyPosition();
float ang;
float dist;
int res;
DebugKick(printf(" isStriaght ball abs pos mod: %f\t dir: %f\n",
Mem->BallAbsolutePosition().mod(), Mem->BallAbsolutePosition().dir()));
DebugKick(printf(" isStriaght ball rel pos mod: %f\t dir: %f\n",
Mem->BallRelativePosition().mod(), Mem->BallRelativePosition().dir()));
DebugKick(printf(" isStriaght btraj mod: %f\t dir: %f\n", btraj.mod(), btraj.dir()));
/* Apply the law of cosines to the anle formed by the player's center(A),
the ball's current position(B), and the ball target position(C).
The angle calculated is ABC */
ang = ACos( (Sqr(EndDist) - Sqr(btraj.mod()) -
Sqr(Mem->BallDistance()))/
(-2 * Mem->BallDistance() * btraj.mod()) );
DebugKick(printf(" isStraight ang: %f\n", ang));
if (fabs(ang) > 90) {
DebugKick(printf(" isStraight: Obtuse!\n"));
LogAction2(120, "is_straight_kick: obtuse angle");
return 1; /* obtuse angle implies definately straight */
}
/* get the height of the triangle, ie how close to the player the ball will
go */
dist = Sin(ang) * Mem->BallDistance();
DebugKick(printf(" isStraight dist: %f\n", dist));
LogAction3(120, "is_straight_kick: %f", dist);
res = (fabs(dist) > closeMarg);
return ( res );
}
Float Quat::GetAngle() const
{
Float _w=w;
if (_w<-1.f) _w=-1.f;
else if (_w>1.f) _w=1.f;
Float s=Sqrt(1.f-_w*_w);
return s>Float_Eps ? ACos(_w)*2.f : 0.f;
}
void HSVColor::makeRandom()
{
while (true)
{
Vector3 v(Random::uniform()*2-1, Random::uniform()*2-1, 0);
if (dist<groupS::r3>::d(v, Vector3::ZERO) > 1.0 || dist<groupS::r3>::d(v, Vector3::ZERO) < .1)
continue;
HSVColor hsv;
if (v.Y() >= 0) c_ = Vector3(ACos(la::normalized(v).Dot(Vector3::UNIT_X)),
v.Length(),
1);
else c_ = Vector3(2*M_PI-ACos(la::normalized(v).Dot(Vector3::UNIT_X)),
v.Length(),
1);
break;
}
}
void Quaternion::ToVectorThetha(Real& thetha,Vector& v)
{
thetha = ACos(this->w);
Real sinThethaReciprocal = 1.0 / Sin(thetha);
v.x = this->x * sinThethaReciprocal;
v.y = this->y * sinThethaReciprocal;
v.z = this->z * sinThethaReciprocal;
thetha *= 2;
}
void QuatToRV(Quat &q)
{
if (q.w<-1.f) q.w=-1.f;
if (q.w>1.f) q.w=1.f;
Float halfang=ACos(q.w);
Float s=Sin(halfang); //Don't try to use sinf here...we already tried, it breaks the quaternion...Need to investigate
if (s>Float_Eps)
{
q.v*=(1.f/s);
}
q.w=halfang*2.f;
}
Quat Quat::operator* ( const Float f) const
{
Float w2=w*w;
if (w2<(1.f-Float_Eps))
{
Float s=Sqrt(1.f-w2);
Vec2f Toto;
ASSERTC_Z((w <= 1.f) && (w >= -1.f),"ACOS will bug with val >1.f");
SinCos(Toto,ACos(w)*f);
Quat r;
r.v=v*(Toto.x/s);
r.w=Toto.y;
return r;
}
return *this;
}
void QuatToRV(const Quat &_q,Vec3f &_v)
{
Quat q=_q;
if (q.w<-1.f) q.w=-1.f;
if (q.w>1.f) q.w=1.f;
Float halfang=ACos(q.w);
Float s=Sin(halfang); //Don't try to use sinf here...we already tried, it breaks the quaternion...Need to investigate
if (s>Float_Eps)
{
q.v*=(1.f/s);
}
q.w=halfang*2.f;
_v=q.v,
_v.CNormalize();
_v*=q.w;
}
inline Big<Exponent, Mantissa> atan2(Big<Exponent, Mantissa> const& y, Big<Exponent, Mantissa> const& x)
{
// return ATan2(y, 2); does not (yet) exist in ttmath...
// See http://en.wikipedia.org/wiki/Atan2
Big<Exponent, Mantissa> const zero(0);
Big<Exponent, Mantissa> const two(2);
if (y == zero)
{
// return x >= 0 ? 0 : pi and pi=2*arccos(0)
return x >= zero ? zero : two * ACos(zero);
}
return two * ATan((sqrt(x * x + y * y) - x) / y);
}
Bool Quat::Maximize( Float f)
{
if (w<-1.f) w=-1.f;
if (w>1.f) w=1.f;
Float s=Sqrt(1.f-w*w);
if (s>Float_Eps)
{
f*=0.5f;
Vec2f Toto;
Float a=ACos(w);
if (a<-f) a=-f;
else if (a>f) a=f;
else return FALSE;
SinCos(Toto,a);
v*=(1.f/s)*Toto.x;
w=Toto.y;
return TRUE;
}
return FALSE;
}
Quaternion Quaternion::SlerpExtraSpins(
float t,
const Quaternion &q1,
const Quaternion &q2,
int extraSpins)
{
float cosA = Dot(q1, q2);
float angle = ACos(cosA);
if (Abs(angle) < EPSILON)
{
return q1;
}
float sinA = Sin(angle);
float phase = PI * extraSpins * t;
float invSinA = 1.0f / sinA;
float coeff0 = Sin((1.0f - t) * angle - phase) * invSinA;
float coeff1 = Sin(t * angle + phase) * invSinA;
return coeff0 * q1 + coeff1 * q2;
}
void Quaternion::ToAngleAxis(float &angle, Vector3 &axis) const
{
// The quaternion representing the rotation is
// q = cos(A / 2) + sin(A / 2) * (x * i + y * j + z * k)
float norm = Norm();
if (norm > EPSILON)
{
angle = 2.0f * ACos(w);
float invMag = InvSqrt(norm);
axis.x = x * invMag;
axis.y = y * invMag;
axis.z = z * invMag;
}
else
{
// angle is 0 (mod 2 * pi), so use the Z axis so 2D will work
angle = 0.0;
axis.x = 0.0;
axis.y = 0.0;
axis.z = 1.0;
}
}
BigNum Vector2::angle()
{
return ACos(x / length());
}
Quaternion::Quaternion(float x, float y, float z, float w) :
#if defined(XO_SSE)
xmm(_mm_set_ps(w, z, y, x))
#else
x(x), y(y), z(z), w(w)
#endif
{
}
Quaternion Quaternion::Inverse() const
{
return Quaternion(*this).MakeInverse();
}
Quaternion& Quaternion::MakeInverse()
{
float magnitude = xo_internal::QuaternionSquareSum(*this);
if (CloseEnough(magnitude, 1.0f, Epsilon))
{
return MakeConjugate();
}
if (CloseEnough(magnitude, 0.0f, Epsilon))
{
return *this;
}
MakeConjugate();
(*(Vector4*)this) /= magnitude;
return *this;
}
Quaternion Quaternion::Normalized() const
{
return Quaternion(*this).Normalize();
}
Quaternion& Quaternion::Normalize()
{
float magnitude = xo_internal::QuaternionSquareSum(*this);
if (CloseEnough(magnitude, 1.0f, Epsilon))
{
return *this;
}
magnitude = Sqrt(magnitude);
if (CloseEnough(magnitude, 0.0f, Epsilon))
{
return *this;
}
(*(Vector4*)this) /= magnitude;
return *this;
}
Quaternion Quaternion::Conjugate() const
{
return Quaternion(*this).MakeConjugate();
}
Quaternion& Quaternion::MakeConjugate()
{
_XO_ASSIGN_QUAT(w, -x, -y, -z);
return *this;
}
void Quaternion::GetAxisAngleRadians(Vector3& axis, float& radians) const
{
Quaternion q = Normalized();
#if defined(XO_SSE)
// todo: don't we need to normalize axis in sse too?
axis.xmm = q.xmm;
#else
axis.x = q.x;
axis.y = q.y;
axis.z = q.z;
axis.Normalize();
#endif
radians = (2.0f * ACos(q.w));
}
inline Big<Exponent, Mantissa> acos(Big<Exponent, Mantissa> const& v)
{
return ACos(v);
}
float Quat::GetAngle() const
{
return 2.0f * ACos( w );
}
