SE_Quat SE_Quat::slerp(SE_Quat& starting, SE_Quat& ending,float t)
{
SE_Quat result;
float cosa = starting.d[0]*ending.d[0] + starting.d[1]*ending.d[1] + starting.d[2]*ending.d[2] + starting.d[3]*ending.d[3];
if (cosa < 0.0f)
{
ending.d[0] = -ending.d[0];
ending.d[1] = -ending.d[1];
ending.d[2] = -ending.d[2];
ending.d[3] = -ending.d[3];
cosa = -cosa;
}
float k0, k1;
if (cosa > 0.9999f)
{
k0 = 1.0f - t;
k1 = t;
}
else
{
float sina = SE_Sqrtf( 1.0f - cosa*cosa );
float a = atan2( sina, cosa );
float invSina = 1.0f / sina;
k0 = SE_Sinf((1.0f - t)*a) * invSina;
k1 = SE_Sinf(t*a) * invSina;
}
result.d[0] = starting.d[0]*k0 + ending.d[0]*k1;
result.d[1] = starting.d[1]*k0 + ending.d[1]*k1;
result.d[2] = starting.d[2]*k0 + ending.d[2]*k1;
result.d[3] = starting.d[3]*k0 + ending.d[3]*k1;
return result;
}
SE_Ray SE_Camera::screenCoordinateToRay(int x, int y)
{
float xp = ((float)x) / (mViewport.right - mViewport.left);
float yp = 1 - ((float)y) / (mViewport.bottom - mViewport.top);
SE_Rect<float> nearRect = mFrustum.getNearPlaneRect();
float xv = (1 - xp) * nearRect.left + xp * nearRect.right;
float yv = (1 - yp) * nearRect.bottom + yp * nearRect.top;
float dirLen = SE_Sqrtf(xv * xv + yv * yv + mFrustum.getNear() * mFrustum.getNear());
SE_Vector3f dir;
dir.x = mAxisX.x * xv / dirLen + mAxisY.x * yv / dirLen + mAxisZ.x * (-mFrustum.getNear()) / dirLen;
dir.y = mAxisX.y * xv / dirLen + mAxisY.y * yv /dirLen + mAxisZ.y * (-mFrustum.getNear()) / dirLen;
dir.z = mAxisX.z * xv / dirLen + mAxisY.z * yv / dirLen + mAxisZ.z * (-mFrustum.getNear()) / dirLen;
return SE_Ray(mLocation, dir, false);
}
static void sphereOfSphereAndPoint(SE_Sphere* s, SE_Vector3f* point)
{
SE_Vector3f d, tmp1, tmp2;
float dist2;
SE_Vec3f_Subtract(point, &s->center, &d);
dist2 = SE_Vec3f_LengthSquare(&d);
if(dist2 > s->radius * s->radius)
{
float dist = SE_Sqrtf(dist2);
float newRadius = (s->radius + dist) * 0.5f;
float k = (newRadius - s->radius) / dist;
s->radius = newRadius;
SE_Vec3f_Mul(&d, k, &tmp1);
SE_Vec3f_Add(&tmp1, &s->center, &tmp2);
SE_Vec3f_Copy(&tmp2, &s->center);
}
}
float SE_Quat::length() const
{
return SE_Sqrtf(w * w + x * x + y * y + z * z);
}
