Vector2 OpenGLRenderer::Project(const Matrix4 &cameraMatrix, const Matrix4 &projectionMatrix, const Polycode::Rectangle &viewport, const Vector3 &coordiante) const {
GLdouble mv[16];
Matrix4 camInverse = cameraMatrix.Inverse();
Matrix4 cmv;
cmv.identity();
cmv = cmv * camInverse;
for(int i=0; i < 16; i++) {
mv[i] = cmv.ml[i];
}
GLint vp[4] = {viewport.x, viewport.y, viewport.w, viewport.h};
GLdouble _sceneProjectionMatrix[16];
for(int i=0; i < 16; i++) {
_sceneProjectionMatrix[i] = projectionMatrix.ml[i];
}
GLdouble coords[3];
gluProject(coordiante.x, coordiante.y, coordiante.z, mv, _sceneProjectionMatrix, vp, &coords[0], &coords[1], &coords[2]);
return Vector2(coords[0] / backingResolutionScaleX, (viewport.h-coords[1]) / backingResolutionScaleY);
}
Vector3 OpenGLRenderer::projectRayFrom2DCoordinate(Number x, Number y, const Matrix4 &cameraMatrix, const Matrix4 &projectionMatrix, const Polycode::Rectangle &viewport) {
GLdouble nearPlane[3],farPlane[3];
GLdouble mv[16];
Matrix4 camInverse = cameraMatrix.Inverse();
Matrix4 cmv;
cmv.identity();
cmv = cmv * camInverse;
for(int i=0; i < 16; i++) {
mv[i] = cmv.ml[i];
}
GLint vp[4] = {viewport.x, viewport.y, viewport.w, viewport.h};
GLdouble _sceneProjectionMatrix[16];
for(int i=0; i < 16; i++) {
_sceneProjectionMatrix[i] = projectionMatrix.ml[i];
}
gluUnProject(x, (yRes*backingResolutionScaleY) - y, 0.0, mv, _sceneProjectionMatrix, vp, &nearPlane[0], &nearPlane[1], &nearPlane[2]);
gluUnProject(x, (yRes*backingResolutionScaleY) - y, 1.0, mv, _sceneProjectionMatrix, vp, &farPlane[0], &farPlane[1], &farPlane[2]);
Vector3 nearVec(nearPlane[0], nearPlane[1], nearPlane[2]);
Vector3 farVec(farPlane[0], farPlane[1], farPlane[2]);
Vector3 dirVec = (farVec) - (nearVec);
dirVec.Normalize();
return dirVec;
}
Vector3 OpenGLRenderer::Unproject(Number x, Number y, const Matrix4 &cameraMatrix, const Matrix4 &projectionMatrix, const Polycode::Rectangle &viewport) {
Vector3 coords;
GLfloat wx, wy, wz;
GLdouble cx, cy, cz;
GLdouble mv[16];
Matrix4 camInverse = cameraMatrix.Inverse();
Matrix4 cmv;
cmv.identity();
cmv = cmv * camInverse;
for(int i=0; i < 16; i++) {
mv[i] = cmv.ml[i];
}
GLint vp[4] = {viewport.x, viewport.y, viewport.w, viewport.h};
GLdouble _sceneProjectionMatrix[16];
for(int i=0; i < 16; i++) {
_sceneProjectionMatrix[i] = projectionMatrix.ml[i];
}
wx = ( Number ) x;
wy = ( Number ) vp[3] - ( Number ) y;
glReadPixels( x * backingResolutionScaleX, wy * backingResolutionScaleY, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &wz );
gluUnProject( wx, wy, wz, mv, _sceneProjectionMatrix, vp, &cx, &cy, &cz );
coords = Vector3( cx, cy, cz );
return coords;
}
void BaseMesh::CreateViewPlane(float EyeDist, UINT slices, MatrixController &MC)
{
Matrix4 Perspective = MC.Perspective, PInverse;
PInverse = Perspective.Inverse();
Vec3f PerspectiveCoord(0.0f, 0.0f, EyeDist);
PerspectiveCoord = Perspective.TransformPoint(PerspectiveCoord); //get the top-left coord in persp. space
PerspectiveCoord.x = 1.0f;
PerspectiveCoord.y = -1.0f;
PerspectiveCoord = PInverse.TransformPoint(PerspectiveCoord); //get the bottom-right coord in persp. space
CreatePlane(2.0f, slices, slices); //create the X-Y plane
Matrix4 Scale = Matrix4::Scaling(Vec3f(PerspectiveCoord.x, PerspectiveCoord.y, 1.0f)); //scale it appropriately,
Matrix4 Translate = Matrix4::Translation(Vec3f(0.0f,0.0f,PerspectiveCoord.z)); //translate it away from the eye,
Matrix4 VInverse = MC.View.Inverse(); //we need to transform our mesh and we want to fact the view and world transforms in
Matrix4 WInverse = MC.World.Inverse();
//
// Translate and scale, then go into object space by multiplying through the inverse of view/world.
//
ApplyMatrix(Translate * Scale * VInverse * WInverse);
}
void Picker::FindHits(const D3D9Mesh &M, const Matrix4 &WorldViewProjectionTransform, const Vec3f &VecEye, const Vec2f &MousePoint,
Vector<float> &Hits, DWORD &FirstHitFaceIndex, float &u, float &v)
{
Vec3f LookPtProjection(Math::LinearMap(0.0f, 1.0f, -1.0f, 1.0f, MousePoint.x),
Math::LinearMap(0.0f, 1.0f, 1.0f, -1.0f, MousePoint.y),
0.5f);
Vec3f LookPtObject = WorldViewProjectionTransform.Inverse().TransformPoint(LookPtProjection);
M.QueryHits(VecEye, LookPtObject - VecEye, Hits, FirstHitFaceIndex, u, v);
}
Vector3 Camera::UnProject(float32 winx, float32 winy, float32 winz, const Rect & viewport)
{
// Matrix4 finalMatrix = modelMatrix * projMatrix;//RenderManager::Instance()->GetUniformMatrix(RenderManager::UNIFORM_MATRIX_MODELVIEWPROJECTION);
Matrix4 finalMatrix = GetUniformProjModelMatrix();
finalMatrix.Inverse();
Vector4 in(winx, winy, winz, 1.0f);
/* Map x and y from window coordinates */
switch(RenderManager::Instance()->GetRenderOrientation())
{
case Core::SCREEN_ORIENTATION_LANDSCAPE_LEFT:
{
float32 xx = (in.y - viewport.y) / viewport.dy;
float32 yy = (in.x - viewport.x) / viewport.dx;
in.x = xx;
in.y = yy;
}
break;
case Core::SCREEN_ORIENTATION_LANDSCAPE_RIGHT:
{
DVASSERT(false);
}
break;
default:
in.x = (in.x - viewport.x) / viewport.dx;
in.y = 1.0f - (in.y - viewport.y) / viewport.dy;
break;
}
/* Map to range -1 to 1 */
in.x = in.x * 2 - 1;
in.y = in.y * 2 - 1;
in.z = in.z * 2 - 1;
Vector4 out = in * finalMatrix;
Vector3 result(0,0,0);
if (out.w == 0.0) return result;
result.x = out.x / out.w;
result.y = out.y / out.w;
result.z = out.z / out.w;
return result;
}
void Frustum::Define(const Matrix4& projection)
{
Matrix4 projInverse = projection.Inverse();
vertices_[0] = projInverse * Vector3(1.0f, 1.0f, 0.0f);
vertices_[1] = projInverse * Vector3(1.0f, -1.0f, 0.0f);
vertices_[2] = projInverse * Vector3(-1.0f, -1.0f, 0.0f);
vertices_[3] = projInverse * Vector3(-1.0f, 1.0f, 0.0f);
vertices_[4] = projInverse * Vector3(1.0f, 1.0f, 1.0f);
vertices_[5] = projInverse * Vector3(1.0f, -1.0f, 1.0f);
vertices_[6] = projInverse * Vector3(-1.0f, -1.0f, 1.0f);
vertices_[7] = projInverse * Vector3(-1.0f, 1.0f, 1.0f);
UpdatePlanes();
}
void Camera::doCameraTransform() {
if(fovSet)
CoreServices::getInstance()->getRenderer()->setFOV(fov);
CoreServices::getInstance()->getRenderer()->setExposureLevel(exposureLevel);
if(matrixDirty) {
rebuildTransformMatrix();
}
Matrix4 camMatrix = getConcatenatedMatrix();
CoreServices::getInstance()->getRenderer()->setCameraMatrix(camMatrix);
camMatrix = camMatrix.Inverse();
CoreServices::getInstance()->getRenderer()->multModelviewMatrix(camMatrix);
}
void Camera::doCameraTransform() {
viewport = renderer->getViewport();
switch (projectionMode) {
case PERSPECTIVE_FOV:
renderer->setViewportShift(cameraShift.x, cameraShift.y);
renderer->setProjectionFromFoV(fov, nearClipPlane, farClipPlane);
renderer->setPerspectiveDefaults();
break;
case PERSPECTIVE_FRUSTUM:
renderer->setProjectionFromFrustum(leftFrustum, rightFrustum, bottomFrustum, topFrustum, nearClipPlane, farClipPlane);
renderer->setPerspectiveDefaults();
break;
case ORTHO_SIZE_MANUAL:
renderer->setProjectionOrtho(orthoSizeX, orthoSizeY, nearClipPlane, farClipPlane, !topLeftOrtho);
break;
case ORTHO_SIZE_LOCK_HEIGHT:
renderer->setProjectionOrtho(orthoSizeY * (viewport.w/viewport.h), orthoSizeY, nearClipPlane, farClipPlane, !topLeftOrtho);
break;
case ORTHO_SIZE_LOCK_WIDTH:
renderer->setProjectionOrtho(orthoSizeX, orthoSizeX * (viewport.h/viewport.w), nearClipPlane, farClipPlane, !topLeftOrtho);
break;
case ORTHO_SIZE_VIEWPORT:
renderer->setProjectionOrtho(viewport.w / renderer->getBackingResolutionScaleX(), viewport.h / renderer->getBackingResolutionScaleY(), !topLeftOrtho);
break;
case MANUAL_MATRIX:
renderer->setProjectionMatrix(projectionMatrix);
break;
}
renderer->setExposureLevel(exposureLevel);
if(projectionMode != MANUAL_MATRIX) {
projectionMatrix = renderer->getProjectionMatrix();
}
if(matrixDirty) {
rebuildTransformMatrix();
}
Matrix4 camMatrix = getConcatenatedMatrix();
renderer->setCameraMatrix(camMatrix);
camMatrix = camMatrix.Inverse();
renderer->multModelviewMatrix(camMatrix);
}
Number Ray::boxIntersect(const Vector3 &box, const Matrix4 &transformMatrix, float near, float far) const {
if(box.x == 0 || box.y == 0 || box.z == 0)
return -1.0;
Ray r = tranformByMatrix(transformMatrix.Inverse());
Vector3 bounds[2];
bounds[0] = Vector3(-box.x * 0.5, -box.y * 0.5, -box.z * 0.5);
bounds[1] = Vector3(box.x * 0.5, box.y * 0.5, box.z * 0.5);
float tmin, tmax, tymin, tymax, tzmin, tzmax;
tmin = (bounds[r.sign[0]].x - r.origin.x) * r.inv_direction.x;
tmax = (bounds[1-r.sign[0]].x - r.origin.x) * r.inv_direction.x;
tymin = (bounds[r.sign[1]].y - r.origin.y) * r.inv_direction.y;
tymax = (bounds[1-r.sign[1]].y - r.origin.y) * r.inv_direction.y;
if ( (tmin > tymax) || (tymin > tmax) )
return -1.0;
if (tymin > tmin)
tmin = tymin;
if (tymax < tmax)
tmax = tymax;
tzmin = (bounds[r.sign[2]].z - r.origin.z) * r.inv_direction.z;
tzmax = (bounds[1-r.sign[2]].z - r.origin.z) * r.inv_direction.z;
if ( (tmin > tzmax) || (tzmin > tmax) )
return -1.0;
if (tzmin > tmin)
tmin = tzmin;
if (tzmax < tmax)
tmax = tzmax;
if( (tmin < far) && (tmax > near) ) {
return fabs(tmin);
} else {
return -1.0;
}
}
void Camera::doCameraTransform() {
Renderer *renderer = CoreServices::getInstance()->getRenderer();
if(!orthoMode) {
renderer->setViewportShift(cameraShift.x, cameraShift.y);
renderer->setFOV(fov);
}
renderer->setExposureLevel(exposureLevel);
projectionMatrix = renderer->getProjectionMatrix();
if(matrixDirty) {
rebuildTransformMatrix();
}
Matrix4 camMatrix = getConcatenatedMatrix();
renderer->setCameraMatrix(camMatrix);
camMatrix = camMatrix.Inverse();
renderer->multModelviewMatrix(camMatrix);
}
void Frustum::DefineSplit(const Matrix4& projection, float near, float far)
{
Matrix4 projInverse = projection.Inverse();
// Figure out depth values for near & far
Vector4 nearTemp = projection * Vector4(0.0f, 0.0f, near, 1.0f);
Vector4 farTemp = projection * Vector4(0.0f, 0.0f, far, 1.0f);
float nearZ = nearTemp.z_ / nearTemp.w_;
float farZ = farTemp.z_ / farTemp.w_;
vertices_[0] = projInverse * Vector3(1.0f, 1.0f, nearZ);
vertices_[1] = projInverse * Vector3(1.0f, -1.0f, nearZ);
vertices_[2] = projInverse * Vector3(-1.0f, -1.0f, nearZ);
vertices_[3] = projInverse * Vector3(-1.0f, 1.0f, nearZ);
vertices_[4] = projInverse * Vector3(1.0f, 1.0f, farZ);
vertices_[5] = projInverse * Vector3(1.0f, -1.0f, farZ);
vertices_[6] = projInverse * Vector3(-1.0f, -1.0f, farZ);
vertices_[7] = projInverse * Vector3(-1.0f, 1.0f, farZ);
UpdatePlanes();
}
void preprocessing(Mesh * _mesh, Vector3 rotate, Vector3 scale, Vector3 translate, std::vector<Vector3 * >& normals )
{
// compute bounding box
Vector3 minPosition;
Vector3 maxPosition;
Vector3 centerPosition;
double radius=0.0;
maxPosition[0]=std::numeric_limits<double>::min();
maxPosition[1]=std::numeric_limits<double>::min();
maxPosition[2]=std::numeric_limits<double>::min();
minPosition[0]=std::numeric_limits<double>::max();
minPosition[1]=std::numeric_limits<double>::max();
minPosition[2]=std::numeric_limits<double>::max();
for(unsigned int i=0;i<_mesh->numberOfVertices();i++)
{
Vector3 point = *(_mesh->getVertex(i)->getPosition());
if(point[0] < minPosition[0])
{
minPosition[0] = point[0];
}
if(point[1] <minPosition[1])
{
minPosition[1] = point[1];
}
if(point[2] <minPosition[2])
{
minPosition[2] = point[2];
}
if(point[0] >maxPosition[0])
{
maxPosition[0] = point[0];
}
if(point[1] >maxPosition[1])
{
maxPosition[1] = point[1];
}
if(point[2] >maxPosition[2])
{
maxPosition[2] = point[2];
}
}
// compute center position
centerPosition[0] = (maxPosition[0] + minPosition[0])/2.0;
centerPosition[1] = (maxPosition[1] + minPosition[1])/2.0;
centerPosition[2] = (maxPosition[2] + minPosition[2])/2.0;
// compute radius
//radius = (maxPosition[0]-centerPosition[0])*(maxPosition[0]-centerPosition[0]) +
// (maxPosition[1]-centerPosition[1])*(maxPosition[1]-centerPosition[1]) +
// (maxPosition[2]-centerPosition[2])*(maxPosition[2]-centerPosition[2]);
//bounding box diagonal length:
double bbdl = (maxPosition-minPosition).length();
// normalize size and translate to origin
for (unsigned int i=0; i < _mesh->numberOfVertices(); i++) {
Vector3 p = *(_mesh->getVertex(i)->getPosition());
p = (p-centerPosition)/bbdl;
_mesh->getVertex(i)->setPosition(p);
}
// transform mesh corresponding to rotate, translate and scale
Matrix4 s;
s.loadScaling(scale);
Matrix4 rx;
rx.loadRotation(Vector3(1.,0.,0.),(M_PI/180.)*rotate.x);
Matrix4 ry;
ry.loadRotation(Vector3(0.,1.,0.),(M_PI/180.)*rotate.y);
Matrix4 rz;
rz.loadRotation(Vector3(0.,0.,1.),(M_PI/180.)*rotate.z);
Matrix4 Tf;
Tf.loadIdentity();
//Tf *= s*rx*ry*rz*t;
Tf = s*rx*ry*rz;
Matrix4 invTft = Tf.Inverse().Transpose();
//Transform points and normals
for (unsigned int i=0; i < _mesh->numberOfVertices(); i++) {
Vector3 p = *(_mesh->getVertex(i)->getPosition());
p = Tf*p + translate;
_mesh->getVertex(i)->setPosition(p);
}
for( std::vector<Vector3*>::iterator it=normals.begin(); it!=normals.end(); ++it)
{
Vector3* n = (*it);
Vector4 n4 = invTft*Vector4(*n,0.);
*n = Vector3(n4.x,n4.y,n4.z);
n->normalize();
//_mesh->getVertex(i)->setNormal(n);
}
}
Plane Plane::Transformed(const Matrix4& transform) const
{
return Plane(transform.Inverse().Transpose() * ToVector4());
}
void Plane::Transform(const Matrix4& transform)
{
Define(transform.Inverse().Transpose() * ToVector4());
}
void ImposterNode::UpdateImposter()
{
Camera * camera = scene->GetCurrentCamera();
Camera * imposterCamera = new Camera();
Vector3 cameraPos = camera->GetPosition();
Entity * child = GetChild(0);
AABBox3 bbox = child->GetWTMaximumBoundingBoxSlow();
Vector3 bboxCenter = bbox.GetCenter();
imposterCamera->Setup(camera->GetFOV(), camera->GetAspect(), camera->GetZNear(), camera->GetZFar());
imposterCamera->SetTarget(bbox.GetCenter());
imposterCamera->SetPosition(cameraPos);
imposterCamera->SetUp(camera->GetUp());
imposterCamera->SetLeft(camera->GetLeft());
Rect viewport = RenderManager::Instance()->GetViewport();
const Matrix4 & mvp = imposterCamera->GetUniformProjModelMatrix();
AABBox3 screenBounds;
GetOOBBoxScreenCoords(child, mvp, screenBounds);
Vector4 pv(bboxCenter);
pv = pv*mvp;
pv.z = (pv.z/pv.w + 1.f) * 0.5f;
float32 bboxCenterZ = pv.z;
Vector2 screenSize = Vector2(screenBounds.max.x-screenBounds.min.x, screenBounds.max.y-screenBounds.min.y);
Vector3 screenBillboardVertices[4];
screenBillboardVertices[0] = Vector3(screenBounds.min.x, screenBounds.min.y, screenBounds.min.z);
screenBillboardVertices[1] = Vector3(screenBounds.max.x, screenBounds.min.y, screenBounds.min.z);
screenBillboardVertices[2] = Vector3(screenBounds.min.x, screenBounds.max.y, screenBounds.min.z);
screenBillboardVertices[3] = Vector3(screenBounds.max.x, screenBounds.max.y, screenBounds.min.z);
center = Vector3();
Matrix4 invMvp = mvp;
invMvp.Inverse();
for(int32 i = 0; i < 4; ++i)
{
//unproject
Vector4 out;
out.x = 2.f*(screenBillboardVertices[i].x-viewport.x)/viewport.dx-1.f;
out.y = 2.f*(screenBillboardVertices[i].y-viewport.y)/viewport.dy-1.f;
out.z = 2.f*screenBillboardVertices[i].z-1.f;
out.w = 1.f;
out = out*invMvp;
DVASSERT(out.w != 0.f);
out.x /= out.w;
out.y /= out.w;
out.z /= out.w;
imposterVertices[i] = Vector3(out.x, out.y, out.z);
center += imposterVertices[i];
}
center /= 4.f;
//draw
RecreateFbo(screenSize);
//Logger::Info("%f, %f", screenSize.x, screenSize.y);
if(!block)
{
return;
}
direction = camera->GetPosition()-center;
direction.Normalize();
distanceSquaredToCamera = (center-cameraPos).SquareLength();
float32 nearPlane = sqrtf(distanceSquaredToCamera);
//float32 farPlane = nearPlane + (bbox.max.z-bbox.min.z);
float32 w = (imposterVertices[1]-imposterVertices[0]).Length();
float32 h = (imposterVertices[2]-imposterVertices[0]).Length();
//TODO: calculate instead of +50
imposterCamera->Setup(-w/2.f, w/2.f, -h/2.f, h/2.f, nearPlane, nearPlane+50.f);
Rect oldViewport = RenderManager::Instance()->GetViewport();
//Texture * target = fbo->GetTexture();
RenderManager::Instance()->AppendState(RenderState::STATE_SCISSOR_TEST);
RenderManager::Instance()->State()->SetScissorRect(Rect(block->offset.x, block->offset.y, block->size.dx, block->size.dy));
RenderManager::Instance()->FlushState();
//TODO: use one "clear" function instead of two
//if(block->size.x == 512.f)
//{
// RenderManager::Instance()->ClearWithColor(0.f, .8f, 0.f, 1.f);
//}
//else if(block->size.x == 256.f)
//{
// RenderManager::Instance()->ClearWithColor(0.f, .3f, 0.f, 1.f);
//}
//else if(block->size.x == 128.f)
//{
// RenderManager::Instance()->ClearWithColor(.3f, .3f, 0.f, 1.f);
//}
//else
//{
// RenderManager::Instance()->ClearWithColor(.3f, 0.f, 0.f, 1.f);
//}
RenderManager::Instance()->ClearWithColor(.0f, .0f, 0.f, .0f);
RenderManager::Instance()->ClearDepthBuffer();
RenderManager::Instance()->RemoveState(RenderState::STATE_SCISSOR_TEST);
RenderManager::Instance()->SetViewport(Rect(block->offset.x, block->offset.y, block->size.dx, block->size.dy), true);
imposterCamera->SetTarget(center);
imposterCamera->Set();
//TODO: remove this call
HierarchicalRemoveCull(child);
RenderManager::Instance()->FlushState();
child->Draw();
RenderManager::Instance()->SetViewport(oldViewport, true);
isReady = true;
state = STATE_IMPOSTER;
//unproject
screenBillboardVertices[0] = Vector3(screenBounds.min.x, screenBounds.min.y, bboxCenterZ);
screenBillboardVertices[1] = Vector3(screenBounds.max.x, screenBounds.min.y, bboxCenterZ);
screenBillboardVertices[2] = Vector3(screenBounds.min.x, screenBounds.max.y, bboxCenterZ);
screenBillboardVertices[3] = Vector3(screenBounds.max.x, screenBounds.max.y, bboxCenterZ);
for(int32 i = 0; i < 4; ++i)
{
//unproject
Vector4 out;
out.x = 2.f*(screenBillboardVertices[i].x-viewport.x)/viewport.dx-1.f;
out.y = 2.f*(screenBillboardVertices[i].y-viewport.y)/viewport.dy-1.f;
out.z = 2.f*screenBillboardVertices[i].z-1.f;
out.w = 1.f;
out = out*invMvp;
DVASSERT(out.w != 0.f);
out.x /= out.w;
out.y /= out.w;
out.z /= out.w;
imposterVertices[i] = Vector3(out.x, out.y, out.z);
}
SafeRelease(imposterCamera);
ClearGeometry();
CreateGeometry();
}
void NaturalSpline1<Real>::CreatePeriodicSpline ()
{
mB = new1<Real>( mNumSegments );
mC = new1<Real>( mNumSegments );
mD = new1<Real>( mNumSegments );
#if 1
// Solving the system using a standard linear solver appears to be
// numerically stable.
const int size = 4 * mNumSegments;
GMatrix<Real> mat( size, size );
GVector<Real> rhs( size );
int i, j, k;
Real delta, delta2, delta3;
for ( i = 0, j = 0; i < mNumSegments - 1; ++i, j += 4 )
{
delta = mTimes[i + 1] - mTimes[i];
delta2 = delta * delta;
delta3 = delta * delta2;
mat[j + 0][j + 0] = ( Real )1;
mat[j + 0][j + 1] = ( Real )0;
mat[j + 0][j + 2] = ( Real )0;
mat[j + 0][j + 3] = ( Real )0;
mat[j + 1][j + 0] = ( Real )1;
mat[j + 1][j + 1] = delta;
mat[j + 1][j + 2] = delta2;
mat[j + 1][j + 3] = delta3;
mat[j + 2][j + 0] = ( Real )0;
mat[j + 2][j + 1] = ( Real )1;
mat[j + 2][j + 2] = ( ( Real )2 ) * delta;
mat[j + 2][j + 3] = ( ( Real )3 ) * delta2;
mat[j + 3][j + 0] = ( Real )0;
mat[j + 3][j + 1] = ( Real )0;
mat[j + 3][j + 2] = ( Real )1;
mat[j + 3][j + 3] = ( ( Real )3 ) * delta;
k = j + 4;
mat[j + 0][k + 0] = ( Real )0;
mat[j + 0][k + 1] = ( Real )0;
mat[j + 0][k + 2] = ( Real )0;
mat[j + 0][k + 3] = ( Real )0;
mat[j + 1][k + 0] = ( Real ) - 1;
mat[j + 1][k + 1] = ( Real )0;
mat[j + 1][k + 2] = ( Real )0;
mat[j + 1][k + 3] = ( Real )0;
mat[j + 2][k + 0] = ( Real )0;
mat[j + 2][k + 1] = ( Real ) - 1;
mat[j + 2][k + 2] = ( Real )0;
mat[j + 2][k + 3] = ( Real )0;
mat[j + 3][k + 0] = ( Real )0;
mat[j + 3][k + 1] = ( Real )0;
mat[j + 3][k + 2] = ( Real ) - 1;
mat[j + 3][k + 3] = ( Real )0;
}
delta = mTimes[i + 1] - mTimes[i];
delta2 = delta * delta;
delta3 = delta * delta2;
mat[j + 0][j + 0] = ( Real )1;
mat[j + 0][j + 1] = ( Real )0;
mat[j + 0][j + 2] = ( Real )0;
mat[j + 0][j + 3] = ( Real )0;
mat[j + 1][j + 0] = ( Real )1;
mat[j + 1][j + 1] = delta;
mat[j + 1][j + 2] = delta2;
mat[j + 1][j + 3] = delta3;
mat[j + 2][j + 0] = ( Real )0;
mat[j + 2][j + 1] = ( Real )1;
mat[j + 2][j + 2] = ( ( Real )2 ) * delta;
mat[j + 2][j + 3] = ( ( Real )3 ) * delta2;
mat[j + 3][j + 0] = ( Real )0;
mat[j + 3][j + 1] = ( Real )0;
mat[j + 3][j + 2] = ( Real )1;
mat[j + 3][j + 3] = ( ( Real )3 ) * delta;
k = 0;
mat[j + 0][k + 0] = ( Real )0;
mat[j + 0][k + 1] = ( Real )0;
mat[j + 0][k + 2] = ( Real )0;
mat[j + 0][k + 3] = ( Real )0;
mat[j + 1][k + 0] = ( Real ) - 1;
mat[j + 1][k + 1] = ( Real )0;
mat[j + 1][k + 2] = ( Real )0;
mat[j + 1][k + 3] = ( Real )0;
mat[j + 2][k + 0] = ( Real )0;
mat[j + 2][k + 1] = ( Real ) - 1;
mat[j + 2][k + 2] = ( Real )0;
mat[j + 2][k + 3] = ( Real )0;
mat[j + 3][k + 0] = ( Real )0;
mat[j + 3][k + 1] = ( Real )0;
mat[j + 3][k + 2] = ( Real ) - 1;
mat[j + 3][k + 3] = ( Real )0;
for ( i = 0, j = 0; i < mNumSegments; ++i, j += 4 )
{
rhs[j + 0] = mA[i];
rhs[j + 1] = ( Real )0;
rhs[j + 2] = ( Real )0;
rhs[j + 3] = ( Real )0;
}
GVector<Real> coeff( size );
bool solved = LinearSystem<Real>().Solve( mat, rhs, coeff );
assertion( solved, "Failed to solve linear system\n" );
WM5_UNUSED( solved );
for ( i = 0, j = 0; i < mNumSegments; ++i )
{
j++;
mB[i] = coeff[j++];
mC[i] = coeff[j++];
mD[i] = coeff[j++];
}
#endif
#if 0
// Solving the system using the equations derived in the PDF
// "Fitting a Natural Spline to Samples of the Form (t,f(t))"
// is ill-conditioned. TODO: Find a way to row-reduce the matrix of the
// PDF in a numerically stable manner yet retaining the O(n) asymptotic
// behavior.
// Compute the inverses M[i]^{-1}.
const int numSegmentsM1 = mNumSegments - 1;
Matrix4<Real>* invM = new1<Matrix4<Real> >( numSegmentsM1 );
Real delta;
int i;
for ( i = 0; i < numSegmentsM1; i++ )
{
delta = mTimes[i + 1] - mTimes[i];
Real invDelta1 = ( ( Real )1 ) / delta;
Real invDelta2 = invDelta1 / delta;
Real invDelta3 = invDelta2 / delta;
Matrix4<Real>& invMi = invM[i];
invMi[0][0] = ( Real )1;
invMi[0][1] = ( Real )0;
invMi[0][2] = ( Real )0;
invMi[0][3] = ( Real )0;
invMi[1][0] = ( ( Real )( -3 ) ) * invDelta1;
invMi[1][1] = ( ( Real )3 ) * invDelta1;
invMi[1][2] = ( Real )( -2 );
invMi[1][3] = delta;
invMi[2][0] = ( ( Real )3 ) * invDelta2;
invMi[2][1] = ( ( Real )( -3 ) ) * invDelta2;
invMi[2][2] = ( ( Real )3 ) * invDelta1;
invMi[2][3] = ( Real )( -2 );
invMi[3][0] = -invDelta3;
invMi[3][1] = invDelta3;
invMi[3][2] = -invDelta2;
invMi[3][3] = invDelta1;
}
// Matrix M[n-1].
delta = mTimes[i + 1] - mTimes[i];
Real delta2 = delta * delta;
Real delta3 = delta2 * delta;
Matrix4<Real> lastM
(
( Real )1, ( Real )0, ( Real )0, ( Real )0,
( Real )1, delta, delta2, delta3,
( Real )0, ( Real )1, ( ( Real )2 )*delta, ( ( Real )3 )*delta2,
( Real )0, ( Real )0, ( Real )1, ( ( Real )3 )*delta
);
// Matrix L.
Matrix4<Real> LMat
(
( Real )0, ( Real )0, ( Real )0, ( Real )0,
( Real )1, ( Real )0, ( Real )0, ( Real )0,
( Real )0, ( Real )1, ( Real )0, ( Real )0,
( Real )0, ( Real )0, ( Real )1, ( Real )0
);
// Vector U.
Vector<4, Real> U( ( Real )1, ( Real )0, ( Real )0, ( Real )0 );
// Initialize P = L and Q = f[n-2]*U.
Matrix4<Real> P = LMat;
const int numSegmentsM2 = mNumSegments - 2;
Vector<4, Real> Q = mA[numSegmentsM2] * U;
// Compute P and Q.
for ( i = numSegmentsM2; i >= 0; --i )
{
// Matrix L*M[i]^{-1}.
Matrix4<Real> LMInv = LMat * invM[i];
// Update P.
P = LMInv * P;
// Update Q.
if ( i > 0 )
{
Q = mA[i - 1] * U + LMInv * Q;
}
else
{
Q = mA[numSegmentsM1] * U + LMInv * Q;
}
}
// Final update of P.
P = lastM - P;
// Compute P^{-1}.
Matrix4<Real> invP = P.Inverse();
// Compute K[n-1].
Vector<4, Real> coeff = invP * Q;
mB[numSegmentsM1] = coeff[1];
mC[numSegmentsM1] = coeff[2];
mD[numSegmentsM1] = coeff[3];
// Back substitution for the other K[i].
for ( i = numSegmentsM2; i >= 0; i-- )
{
coeff = invM[i] * ( mA[i] * U + LMat * coeff );
mB[i] = coeff[1];
mC[i] = coeff[2];
mD[i] = coeff[3];
}
delete1( invM );
#endif
}
void BoneMoveable::OnUpdateWorldInverseMatrix(Matrix4& transform, int32 dirtyFlag)
{
transform = mWorldMatrix.Value();
transform.Inverse();
}