void NodeControlViewImpl::RotateDelta(float yaw, float pitch, float roll)
{
collada::Property* pProp = m_transform;
float3 coi = m_coi;
float4x4 m;
pProp->GetValue(m);
MatrixTranspose(&m, &m);
float4x4 w = m_node->GetWorld(NULL);
float4x4 wI = m_node->GetWorldI(NULL);
float4x4 mcoi(w);
mcoi(3,0) = coi.x;
mcoi(3,1) = coi.y;
mcoi(3,2) = coi.z;
float4x4 toCOI = mcoi*wI;
float4x4 fromCOI;
MatrixInverse(&fromCOI, 0, &toCOI);
float4x4 ypr;
MatrixRotationYawPitchRoll(&ypr, yaw, pitch, roll);
float4x4 result = fromCOI*ypr*toCOI;
MatrixMultiply(&m, &result, &m);
float4x4 m0 = m;
MatrixTranspose(&m, &m);
pProp->SetValue(m);
// RemoveRoll(&coi);
}
void ClipPlanesSetUp()
{
float4x4 projinvtrans = MatrixTranspose(Inverse(Proj));
float df = dot(-camera.orientation.zdir(),lightposn-camera.position) + lightradius ; // debug tip: use fraction of radius to see hard boundary
float4 pf= float4(0,0,1,df) * projinvtrans;
g_pd3dDevice->SetClipPlane(0,(float*)&pf);
float dn = dot(-camera.orientation.zdir(),lightposn-camera.position) - lightradius ;
float4 pn= float4(0,0,-1,-dn) * projinvtrans;
g_pd3dDevice->SetClipPlane(1,(float*)&pn);
float4x4 viewprojinvtrans = MatrixTranspose(Inverse(ViewProj));
if(renderclipitbox)
for(int i=0;i<6;i++)
{
float4 p;
p.w = -lightradius;
p.xyz()[i/2]=(i%2)?1.0f:-1.0f;
p.w = -dot(p.xyz(),lightposn) + lightradius; // debug visualize with reduced: lightradius/2.0f;
float4 pclip = p * viewprojinvtrans;
g_pd3dDevice->SetClipPlane(i,(float*)&pclip);
}
if(renderclipit)
{
g_pd3dDevice->SetRenderState(D3DRS_CLIPPLANEENABLE ,renderclipitbox?63:3);
}
}
void NodeControlViewImpl::TranslateDelta(float x, float y, float z)
{
assert(m_transform->GetSize() == 16);
float4x4 m;
m_transform->GetValue(m);
MatrixTranspose(&m, &m);
float4x4 t;
MatrixTranslation(&t, x * m_scale, y * m_scale, z * m_scale);
MatrixTranspose(&m, &(t*m));
m_transform->SetValue(m);
}
//-----------------------------------------------------------------------------
// Loads the model * view matrix into pixel shader constants
//-----------------------------------------------------------------------------
void CBaseVSShader::LoadModelViewMatrixIntoVertexShaderConstant( int vertexReg )
{
VMatrix view, model, modelView, transpose;
s_pShaderAPI->GetMatrix( MATERIAL_MODEL, model.m[0] );
MatrixTranspose( model, model );
s_pShaderAPI->GetMatrix( MATERIAL_VIEW, view.m[0] );
MatrixTranspose( view, view );
MatrixMultiply( view, model, modelView );
s_pShaderAPI->SetVertexShaderConstant( vertexReg, modelView.m[0], 3 );
}
void ArrayTranspose(array* t1, array* t2)
{
size_t i;
for(i = 0; i < t1->order; i++){
MatrixTranspose(t1->m[i], t2->m[i]);
}
}
bool SolveNormalEquation(const taucs_ccs_matrix * A, const taucsType * b, taucsType * x) {
taucs_ccs_matrix * ATmat = MatrixTranspose(A);
if (! ATmat) {
return false;
}
taucs_ccs_matrix * ATAmat = Mul2NonSymmMatSymmResult(ATmat,A);
taucsType * rhs = new taucsType[A->n];
MulNonSymmMatrixVector(ATmat, b, rhs);
// solve the system
int rc = taucs_linsolve(ATAmat,
NULL,
1,
x,
rhs,
SIVANfactor,
SIVANopt_arg);
taucs_ccs_free(ATmat);
taucs_ccs_free(ATAmat);
delete[] rhs;
if (rc != TAUCS_SUCCESS) {
return false;
} else {
return true;
}
}
Matrix MatrixInverse( Matrix MatrixA )
{
Matrix InverseMatrix;
Matrix InverseMatrixResult;
vector<complex<double> > Bvector;
complex<double> tempcom0(0);
complex<double> tempcom1(1);
for(int inum = 0;inum != MatrixA.size();++inum)
{
Bvector.clear();
for(int i = 0;i != MatrixA.size();++i)
{
if(i == inum)
{
Bvector.push_back(tempcom1);
}
else
{
Bvector.push_back(tempcom0);
}
}
InverseMatrix.push_back(LUP_Solve(MatrixA,Bvector));
}
InverseMatrixResult = MatrixTranspose(InverseMatrix);
return InverseMatrixResult;
}
void Camera::LocalRotate( float x, float y )
{
Matrix4 viewSpaceRotation;
MakeRotationMatrixX(viewSpaceRotation, x);
Matrix4 viewSpaceRotationY;
MakeRotationMatrixY(viewSpaceRotationY, y);
MatrixMultiply(viewSpaceRotation, viewSpaceRotation, viewSpaceRotationY);
Vector4& newViewDirection = viewSpaceRotation.Transform(Vector3(0.0f, 0.0f, 1.0f));
Matrix4 cameraRotation = GetViewMatrix();
cameraRotation.m41 = cameraRotation.m42 = cameraRotation.m43 = 0.0f;
Matrix4 inverseCameraRotation;
MatrixTranspose(inverseCameraRotation, cameraRotation);
Vector4& newWorldSpaceViewDir = inverseCameraRotation.Transform(newViewDirection);
Vector3 newWorldSpaceViewDir3;
newWorldSpaceViewDir3.x = newWorldSpaceViewDir.x;
newWorldSpaceViewDir3.y = newWorldSpaceViewDir.y;
newWorldSpaceViewDir3.z = newWorldSpaceViewDir.z;
newWorldSpaceViewDir3.Normalize();
m_position = m_lookAt - newWorldSpaceViewDir3 * m_distanceToLookAt;
Vector3 right = m_up.Cross(newWorldSpaceViewDir3);
m_up = newWorldSpaceViewDir3.Cross(right);
m_up.Normalize();
m_viewMatrixDirty = true;
}
static void GL_MultMatrixTranspose(float m[4][4])
{
float t[4][4];
MatrixTranspose(t, m);
glMultMatrixf((float*)t);
}
static void GL_LoadMatrixTranspose(float m[4][4])
{
float t[4][4];
MatrixTranspose(t, m);
glLoadMatrixf((float*)t);
}
/*
* The idea is to push all the prepared on client side to pipeline before draw():
* i.e. this needs to be called just before drawing
* Delivers current matrix stack snapshot to shader (through matrix uniforms @sa vert and frag shader programs),
* updates uniform and vertex variables used by shader so that shader has all needed info
* before draw method (following this).
*/
void PiperGL20::setupChangedVariables()
{
MATRIX mView;
MatrixIdentity(mView);
const MATRIX &mViewProjection = modeMatrix[Piper::PROJECTION].top();
const MATRIX &mModel = modeMatrix[Piper::MODEL].top();
const MATRIX &mModelView = mModel;
// not used - view is always identity
// MatrixMultiply(mViewProjection, mView, modeMatrix[Piper::PROJECTION].top());
// MatrixMultiply(mModelView, mModel, mView);
// Calculate model view projection matrix
MATRIX mMVP;
MatrixMultiply(mMVP, mModel, mViewProjection);
MATRIX mTIM;
MatrixInverse(mTIM, mModel);
MatrixTranspose(mTIM, mTIM);
ShaderData *shader = shaderManager->shaderAt(active_shader);
// Then passes the matrix to that variable
glUniformMatrix4fv( shader->PMVMatrixHandle, 1, GL_FALSE, mMVP.f);
glUniformMatrix4fv( shader->MVMatrixHandle, 1, GL_FALSE, mModelView.f);
glUniformMatrix4fv( shader->ModelMatrixHandle, 1, GL_FALSE, mModel.f);
glUniformMatrix4fv( shader->ViewMatrixHandle, 1, GL_FALSE, mView.f);
glUniformMatrix4fv( shader->TIMMatrixHandle, 1, GL_FALSE, mTIM.f);
if (memcmp(color, previousSetColor, 4 * sizeof(GLfloat))) {
glUniform4f(shader->ColorHandle, color[0], color[1], color[2], color[3]);
memcpy(previousSetColor, color, 4 * sizeof(GLfloat));
}
}
// Kalman Filter Prediction Step
// We "predict" what the next data will be
// What this means is the filter's mean will change
// but we loose "certainty" about where the data is
// (aka. the covariance will increase in this step)
void KalmanPredict(struct KalmanFilter *kal)
{
struct Matrix x_next, P_next, F_trans;
CreateBlankMatrix(x_next);
CreateBlankMatrix(P_next);
CreateBlankMatrix(F_trans);
// Calculate x_next (update guassian mean)
MatrixMult(&x_next, kal->updateMatrix, kal->meanVector);
MatrixAdd(&x_next, x_next, kal->moveVector);
// Calculate p_next (update guassian covariance);
MatrixMult(&P_next, kal->updateMatrix, kal->covarianceMatrixX);
MatrixTranspose(&F_trans, kal->updateMatrix);
MatrixMult(&P_next, P_next, F_trans);
// Copy results to the kalmanfilter class
CopyMatrixByValue(&kal->meanVector, x_next);
CopyMatrixByValue(&kal->covarianceMatrixX, P_next);
DeleteMatrix(&x_next);
DeleteMatrix(&P_next);
DeleteMatrix(&F_trans);
}
//-----------------------------------------------------------------------------
// Loads the projection matrix into pixel shader constants
//-----------------------------------------------------------------------------
void CBaseVSShader::LoadProjectionMatrixIntoVertexShaderConstant( int vertexReg )
{
VMatrix mat, transpose;
s_pShaderAPI->GetMatrix( MATERIAL_PROJECTION, mat.m[0] );
MatrixTranspose( mat, transpose );
s_pShaderAPI->SetVertexShaderConstant( vertexReg, transpose.m[0], 4 );
}
//-----------------------------------------------------------------------------
// Loads the view matrix into vertex shader constants
//-----------------------------------------------------------------------------
void CBaseVSShader::LoadViewMatrixIntoVertexShaderConstant( int vertexReg )
{
VMatrix mat, transpose;
s_pShaderAPI->GetMatrix( MATERIAL_VIEW, mat.m[0] );
MatrixTranspose( mat, transpose );
s_pShaderAPI->SetVertexShaderConstant( vertexReg, transpose.m[0], 3 );
}
bool Terrain::Draw(RenderContext& renderContext)
{
renderContext.ApplyShader(m_useShaderType);
XCShaderHandle* shaderHandle = nullptr;
// Set constants
ICamera& cam = renderContext.GetGlobalShaderData().m_camera;
PerObjectBuffer perObject = {
MatrixTranspose(m_World).GetUnaligned(),
MatrixTranspose(m_World * cam.GetViewMatrix() * cam.GetProjectionMatrix()).GetUnaligned(),
MatrixInverseTranspose(m_World).GetUnaligned(),
XCMatrix().GetUnaligned(),
m_material
};
m_pCBPerObject->UploadDataOnGPU(renderContext.GetDeviceContext(), &perObject, sizeof(PerObjectBuffer));
if (m_useShaderType == ShaderType_LightTexture)
{
shaderHandle = (XCShaderHandle*)renderContext.GetShader(ShaderType_LightTexture);
shaderHandle->SetConstantBuffer("PerObjectBuffer", renderContext.GetDeviceContext(), *m_pCBPerObject);
shaderHandle->SetResource("gDiffuseMap", renderContext.GetDeviceContext(), m_textures[0]);
}
else if (m_useShaderType == ShaderType_TerrainMultiTexture)
{
shaderHandle = (XCShaderHandle*)renderContext.GetShader(ShaderType_TerrainMultiTexture);
shaderHandle->SetConstantBuffer("PerObjectBuffer", renderContext.GetDeviceContext(), *m_pCBPerObject);
shaderHandle->SetResource("gDiffuseMap", renderContext.GetDeviceContext(), m_textures[0]);
shaderHandle->SetResource("gDiffuseMap1", renderContext.GetDeviceContext(), m_textures[1]);
shaderHandle->SetResource("gDiffuseMap2", renderContext.GetDeviceContext(), m_textures[2]);
shaderHandle->SetResource("gBlendMap", renderContext.GetDeviceContext(), m_textures[3]);
}
#if defined(FORWARD_LIGHTING)
XCLightManager* lightMgr = (XCLightManager*)&SystemLocator::GetInstance()->RequestSystem("LightsManager");
shaderHandle->SetConstantBuffer("cbLightsPerFrame", renderContext.GetDeviceContext(), lightMgr->GetLightConstantBuffer());
#endif
shaderHandle->SetVertexBuffer(renderContext.GetDeviceContext(), &m_vertexPosNormTexBuffer);
shaderHandle->SetIndexBuffer(renderContext.GetDeviceContext(), m_indexBuffer);
renderContext.DrawIndexedInstanced(m_indexBuffer.m_indexData.size(), m_indexBuffer.GetIndexBufferInGPUMem());
return true;
}
void NodeControlViewImpl::RotateDeltaFP(float yaw, float pitch, float roll)
{
collada::Property* pProp = m_transform;
if( pProp )
{
assert(pProp->GetSize()==16);
float4x4 m;
pProp->GetValue(m);
MatrixTranspose(&m, &m);
float4x4 ypr;
MatrixRotationYawPitchRoll(&ypr, yaw, pitch, roll);
MatrixTranspose(&m, &(ypr*m));
pProp->SetValue(m);
// RemoveRoll();
}
}
//-----------------------------------------------------------------------------------
void Matrix4x4::MatrixInvertOrthogonal(Matrix4x4* matrix)
{
Vector3 translation = MatrixGetOffset(matrix);
MatrixTranspose(matrix);
MatrixSetColumn(matrix, 3, Vector4(0.0f, 0.0f, 0.0f, 1.0f));
Matrix4x4 translationMatrix;
MatrixMakeTranslation(&translationMatrix, translation);
MatrixInvert(&translationMatrix);
MatrixMultiply(matrix, &translationMatrix, matrix);
}
// Kalman Update Step
// We "update" given what we get for data
// The filter will use the data given to lower our
// uncertainty (aka. covariance)
void KalmanUpdate(struct KalmanFilter *kal, struct Matrix meas)
{
struct Matrix y, S, extTran, K, Sinv, x_next, P_next;
CreateBlankMatrix(y);
CreateBlankMatrix(S);
CreateBlankMatrix(extTran);
CreateBlankMatrix(K);
CreateBlankMatrix(Sinv);
CreateBlankMatrix(x_next);
CreateBlankMatrix(P_next);
CopyMatrix(&kal->measurementVector, meas);
// Find the difference between the move (measurement)
// and what we predicted (extraction * data)
MatrixMult(&y, kal->extractionMatrix, kal->meanVector);
MatrixSub(&y, kal->measurementVector, y);
// The Covariance of the move
MatrixMult(&S, kal->extractionMatrix, kal->covarianceMatrixX);
MatrixTranspose(&extTran, kal->extractionMatrix);
MatrixMult(&S, S, extTran);
MatrixAdd(&S, S, kal->covarianceMatrixZ);
// Kalman Gain
MatrixInv(&Sinv, S);
MatrixMult(&K, kal->covarianceMatrixX, extTran);
MatrixMult(&K, K, Sinv);
// Figure out mean and covariance results
MatrixMult(&x_next, K, y);
MatrixAdd(&x_next, kal->meanVector, x_next);
MatrixMult(&P_next, kal->covarianceMatrixX, extTran);
MatrixMult(&P_next, P_next, Sinv);
MatrixMult(&P_next, P_next, kal->extractionMatrix);
MatrixMult(&P_next, P_next, kal->covarianceMatrixX);
MatrixSub(&P_next, kal->covarianceMatrixX, P_next);
// Copy results to the kalmanfilter class
CopyMatrixByValue(&kal->meanVector, x_next);
CopyMatrixByValue(&kal->covarianceMatrixX, P_next);
// Delete matricies so we don't have memory leaks..
DeleteMatrix(&y);
DeleteMatrix(&S);
DeleteMatrix(&extTran);
DeleteMatrix(&K);
DeleteMatrix(&Sinv);
DeleteMatrix(&x_next);
DeleteMatrix(&P_next);
}
void ParticleData::MatrixMap(float x, float y, float z, float theta, float size, bool init, float speed){
MATRIX mov;
MATRIX rot;
//シェーダーのコンスタントバッファーに各種データを渡す
D3D11_MAPPED_SUBRESOURCE pData;
CONSTANT_BUFFER_P cb;
dx->pDeviceContext->Map(pConstantBuffer, 0, D3D11_MAP_WRITE_DISCARD, 0, &pData);
MatrixRotationZ(&rot, theta);
MatrixTranslation(&mov, x, y, z);
MatrixMultiply(&dx->World, &rot, &mov);
MatrixMultiply(&cb.WV, &dx->World, &dx->View);
cb.Proj = dx->Proj;
MatrixTranspose(&cb.WV);
MatrixTranspose(&cb.Proj);
cb.size.x = size;
if (init)cb.size.y = 1.0f; else cb.size.y = 0.0f;
cb.size.z = speed;
memcpy_s(pData.pData, pData.RowPitch, (void*)(&cb), sizeof(cb));
dx->pDeviceContext->Unmap(pConstantBuffer, 0);
}
/*------------------------------------------------------------
ContrastVMF() - computes the variance multiplication factor
------------------------------------------------------------*/
static double ContrastVMF(MATRIX *X, MATRIX *C) {
float vmf;
MATRIX *Xt, *XtX, *iXtX, *CiXtX, *Ct, *CiXtXCt;
Xt = MatrixTranspose(X,NULL);
XtX = MatrixMultiply(Xt,X,NULL);
iXtX = MatrixInverse(XtX,NULL);
CiXtX = MatrixMultiply(C,iXtX,NULL);
Ct = MatrixTranspose(C,NULL);
CiXtXCt = MatrixMultiply(CiXtX,Ct,NULL);
vmf = CiXtXCt->rptr[1][1];
MatrixFree(&Xt);
MatrixFree(&XtX);
MatrixFree(&iXtX);
MatrixFree(&CiXtX);
MatrixFree(&Ct);
MatrixFree(&CiXtXCt);
return(vmf);
}
int FB_Jolt( void )
{
static float checktime = 0.0F;
//static float jolt_time = 0.0F;
GLOBALSHIP *s;
OBJECT *obj;
VECTOR jolt;
float magnitude, duration;
float attack_level, attack_time;
float fade_level, fade_time;
MATRIX InvMat;
if ( !FF_Supported( FB_Joystick ) )
return 0;
s = &Ships[ WhoIAm ];
obj = &s->Object;
if ( framelag )
magnitude = VectorLength( &JoltForce ) / framelag;
else
magnitude = 0.0F;
magnitude /= MAXTURBOSPEED;
if ( magnitude > 0.01F )
{
MatrixTranspose( &obj->Mat, &InvMat );
ApplyMatrix( &InvMat, &JoltForce, &jolt );
duration = 0.25F;
attack_level = (float) sqrt( magnitude );
attack_time = 0.1F;
fade_level = 0.0F;
fade_time = 0.15F;
if ( magnitude > MAX_MAGNITUDE )
magnitude = MAX_MAGNITUDE;
if ( attack_level > MAX_MAGNITUDE )
attack_level = MAX_MAGNITUDE;
FF_UpdateConstantEffect( FB_Joystick, FF_EFFECT_Jolt,
magnitude, duration,
attack_level, attack_time,
fade_level, fade_time,
&jolt, DIEP_START );
checktime -= framelag;
if ( checktime < 0.0F )
{
checktime = JOLT_CHECK_INTERVAL;
FF_CheckEffectPlaying( FB_Joystick, FF_EFFECT_Jolt );
}
}
JoltForce.x = 0.0F;
JoltForce.y = 0.0F;
JoltForce.z = 0.0F;
return 1;
}
void NodeControlViewImpl::RemoveRoll(const float *pCOI3)
{
collada::Property* pProp = m_transform;
float4x4 w2 = m_node->GetWorld(NULL);
float4x4 upA = m_context->GetUpAxis();
float4x4 w2Up = w2*upA;
float4x4 rot;
float xz2 = sqrt(w2Up._11*w2Up._11+w2Up._13*w2Up._13);
float alpha = -atan(w2Up._12/xz2);
MatrixRotationZ(&rot, alpha);
float4x4 m0;
pProp->GetValue(m0);
MatrixTranspose(&m0, &m0);
if( pCOI3 )
{
float4x4 mcoi(w2);
mcoi(3,0) = pCOI3[0]; // x
mcoi(3,1) = pCOI3[1]; // y
mcoi(3,2) = pCOI3[2]; // z
float4x4 wI = m_node->GetWorldI(NULL);
float4x4 toCOI = mcoi*wI;
float4x4 fromCOI;
MatrixInverse(&fromCOI, 0, &toCOI);
rot = fromCOI*rot*toCOI;
}
float4x4 m = rot*m0;
MatrixTranspose(&m, &m);
pProp->SetValue(m);
}
/* x are the indipendent value of the polynomial equation
y are the dependent value
*/
void OrdinaryLeastSquares(matrix *x, dvector *y, dvector *coefficients)
{
/*y->size = x->row
*
* Z' = n x m => n = x->col; m = x->row
*
* Z'Z = m x n * n x m = m x m => m = x->row
*
* Z'y =
*
*
*/
matrix *x_t; /* Z' that is x transposed */
matrix *x_x_t; /* Z'*Z is the product between x and x_t */
matrix *x_x_t_i; /* x_x_t_i inverted matrix (Z' * Z')^-1 */
dvector *x_t_y; /* Z'* y = x->col */
/* transpose x to x_t*/
NewMatrix(&x_t, x->col, x->row);
MatrixTranspose(x, x_t);
/* Z'*Z */
NewMatrix(&x_x_t, x->col, x->col);
MatrixDotProduct(x_t, x, x_x_t);
/* (Z'*Z)^-1 */
initMatrix(&x_x_t_i);
MatrixInversion(x_x_t, &x_x_t_i);
/* create a vector for store the results of Z'y */
NewDVector(&x_t_y, x->col);
/* Z'y */
MatrixDVectorDotProduct(x_t, y, x_t_y);
/* final data coefficient value */
DVectorResize(&coefficients, x->col);
MatrixDVectorDotProduct(x_x_t_i, x_t_y, coefficients);
/*
for(i = 0; i < 10; i++)
printf("%f\n", coefficients->data[i]);
*/
DelDVector(&x_t_y);
DelMatrix(&x_x_t_i);
DelMatrix(&x_x_t);
DelMatrix(&x_t);
}
/*-----------------------------------------------------*/
DVT *DVTtranspose(DVT *dvt) {
DVT *dvtt;
int r,c;
dvtt = DVTalloc(dvt->D->cols,dvt->D->rows,dvt->dvtfile);
for (c = 0; c < dvtt->D->cols; c++)
DVTsetName(dvtt,c,dvt->RowNames[c],2);
for (r = 0; r < dvtt->D->rows; r++)
DVTsetName(dvtt,r,dvt->ColNames[r],1);
MatrixTranspose(dvt->D, dvtt->D);
dvtt->transposed = !dvt->transposed;
return(dvtt);
}
void TransTransformBack(
VECTOR4 * const pOut,
const VECTOR4 * const pV,
const MATRIX * const pM)
{
VERTTYPE *ppfRows[4];
VERTTYPE pfIn[20];
int i;
const MATRIX *pMa;
#if defined(BUILD_OGL) || defined(BUILD_OGLES) || defined(BUILD_OGLES2)
MATRIX mT;
MatrixTranspose(mT, *pM);
pMa = &mT;
#else
pMa = pM;
#endif
for(i = 0; i < 4; ++i)
{
/*
Set up the array of pointers to matrix coefficients
*/
ppfRows[i] = &pfIn[i * 5];
/*
Copy the 4x4 matrix into RHS of the 5x4 matrix
*/
memcpy(&ppfRows[i][1], &pMa->f[i * 4], 4 * sizeof(float));
}
/*
Copy the "result" vector into the first column of the 5x4 matrix
*/
ppfRows[0][0] = pV->x;
ppfRows[1][0] = pV->y;
ppfRows[2][0] = pV->z;
ppfRows[3][0] = pV->w;
/*
Solve a set of 4 linear equations
*/
MatrixLinearEqSolve(&pOut->x, ppfRows, 4);
}
void Camera::LocalMove( float x, float y, float z )
{
Matrix4 rotationMatrix = GetViewMatrix();
rotationMatrix.m41 = rotationMatrix.m42 = rotationMatrix.m43 = 0.0f;
Matrix4 inverseRotationMatrix;
MatrixTranspose(inverseRotationMatrix, rotationMatrix);
Vector4 worldSpaceOffset = inverseRotationMatrix.Transform(Vector4(x, y, z, 1.0f));
m_position.x += worldSpaceOffset.x;
m_position.y += worldSpaceOffset.y;
m_position.z += worldSpaceOffset.z;
m_lookAt.x += worldSpaceOffset.x;
m_lookAt.y += worldSpaceOffset.y;
m_lookAt.z += worldSpaceOffset.z;
m_viewMatrixDirty = true;
}
CameraInstance* CreateDefaultCamera(EditableScene *es, const char *camTag, const char *nodeTag, const char *instTag)
{
Camera *pCamera = es->CreateCamera();
pCamera->SetTag(camTag);
Node *pNode = es->CreateNode(NULL);
pNode->SetTag(nodeTag);
CameraInstance *pCI = es->CreateCameraInstance(pNode, pCamera);
pCI->SetTag(instTag);
Property* pTransform = pNode->AppendTransform(Transform::MATRIX, Zero)->GetDelegate(NULL);
AutoPtr<AccessProviderLocal> providerLocal;
CreateAccessProviderLocal(&providerLocal);
float3 aabb(es->GetSceneAABB(-1, 0, providerLocal));
float3 center(es->GetSceneCenter(-1, 0, providerLocal));
float4x4 ypr;
MatrixRotationYawPitchRoll(&ypr, 3.141692f/4, -3.141692f/4, 0);
float4x4 t0;
MatrixTranslation(&t0, center.x, center.y, center.y);
float4x4 t1;
float bbD = Vec3Length(&aabb);
MatrixTranslation(&t1, 0, 0, bbD*2);
float4x4 up = es->GetUpAxis();
float4x4 upI;
MatrixInverse(&upI, 0, &up);
float4x4 minusZ;
MatrixScaling(&minusZ, 1,1,-1);
float4x4 m = t1*ypr*t0*minusZ*upI;
MatrixTranspose(&m, &m);
pTransform->SetValue(m);
pCamera->SetFar(10*bbD);
return pCI;
}
/*!\rst
Check that matrix-transpose works.
\return
number of entries where ``A`` and ``A^T`` do not match
\endrst*/
OL_WARN_UNUSED_RESULT int TestMatrixTranspose() noexcept {
const int size_m = 3;
const int size_n = 5;
int total_errors = 0;
std::vector<double> matrix(size_m*size_n);
std::vector<double> matrix_T(size_m*size_n);
std::vector<double> product_matrix(size_n*size_n);
UniformRandomGenerator uniform_generator(34187);
BuildRandomVector(size_m*size_n, -1.0, 1.0, &uniform_generator, matrix.data());
MatrixTranspose(matrix.data(), size_m, size_n, matrix_T.data());
for (int j = 0; j < size_n; ++j) {
for (int i = 0; i < size_m; ++i) {
if (CheckDoubleWithin(matrix[j*size_m + i], matrix_T[i*size_n + j], 0.0) == false) {
++total_errors;
break;
}
}
}
return total_errors;
}
int main()
{
int size=6;
int dim,i,j;
int a[6][6], b[6][6], result[6][6];
printf("Please enter a dimension equal or less than 6: ");
scanf("%5d",&dim);
while(dim>6)
{
printf("Please reenter a dimension equal or less than 6: ");
scanf("%5d",&dim);
}
//a
printf("Please enter the values for matrix a:\n");
for (i=0;i<dim;i++)
{
for (j=0;j<dim;j++)
{
printf("[i] [j] (seperate by hitting enter) = ");
scanf("%5d",&a[i][j]);
}
printf("\n");
}
//b
printf("Please enter the values for matrix b:\n");
for (i=0;i<dim;i++)
{
for (j=0;j<dim;j++)
{
printf("[i] [j] (seperate by hitting enter) = ");
scanf("%5d",&b[i][j]);
}
printf("\n");
}
//print a
printf("Matrix a:\n");
printMatrix(a, dim);
//print b
printf("Matrix b:\n");
printMatrix(b, dim);
//compute and print sum.
printf("----------------------------------");
printf("\nAddition a + b:\n\n");
MatrixAddition(a,b,result,dim);
printf("resulting matrix\n\n");
printMatrix(result, dim);
//compute and print difference between a and b meaning a - b
printf("----------------------------------");
printf("\nSubtraction a - b:\n\n");
MatrixSubtraction(a,b,result,dim);
printf("resulting matrix\n\n");
printMatrix(result, dim);
//compute and print difference between b and a meaning b - a
printf("----------------------------------");
printf("\nSubtraction b - a:\n\n");
MatrixSubtraction(b,a,result,dim);
printf("resulting matrix\n\n");
printMatrix(result, dim);
// multiplication for a * b.
printf("----------------------------------");
printf("\nMultiplication a * b:\n\n");
MatrixMultiplication(a,b,result,dim);
printf("resulting matrix\n\n");
printMatrix(result,dim);
// multiplication for b * a.
printf("----------------------------------");
printf("\nMultiplication b * a:\n\n");
MatrixMultiplication(b,a,result,dim);
printf("resulting matrix\n\n");
printMatrix(result,dim);
// Transpose Matrix a.
printf("----------------------------------");
printf("\nTransposing matrix a:");
MatrixTranspose(a,result,dim);
printf("resulting matrix\n\n");
printMatrix(result,dim);
// Transpose Matrix b.
printf("----------------------------------");
printf("\nTransposing matrix b:");
MatrixTranspose(b,result,dim);
printf("resulting matrix\n\n");
printMatrix(result,dim);
return 0;
}
/* last column must be an integer column which define the classes */
void LDA(matrix *mx, uivector *y, LDAMODEL *lda)
{
size_t i, j, l, k, cc, imin, imax;
array *classes;
array *S;
matrix *X, *X_T, *Sb, *Sw, *InvSw_Sb;
dvector *mutot;
dvector *classmu;
dvector *evect_, *ldfeature;
matrix *covmx;
lda->nclass = 0;
imin = imax = y->data[0];
for(i = 1; i < y->size; i++){
if(y->data[i] > imax){
imax = y->data[i];
}
if(y->data[i] < imin){
imin = y->data[i];
}
}
/* get the number of classes */
if(imin == 0){
lda->class_start = 0;
lda->nclass = imax + 1;
}
else{
lda->class_start = 1;
lda->nclass = imax;
}
/*printf("nclass %d\n", (int)lda->nclass);*/
/* Copy data */
NewMatrix(&X, mx->row, mx->col);
MatrixCopy(mx, &X);
MatrixCheck(X);
/*
for(j = 0; j < mx->col-1; j++){
for(i = 0; i < mx->row; i++){
X->data[i][j] = mx->data[i][j];
}
}
*/
/*create classes of objects */
NewArray(&classes, lda->nclass);
UIVectorResize(&lda->classid, mx->row);
j = 0;
if(imin == 0){
for(k = 0; k < lda->nclass; k++){
cc = 0;
for(i = 0; i < X->row; i++){
if(y->data[i] == k)
cc++;
else
continue;
}
NewArrayMatrix(&classes, k, cc, X->col);
cc = 0;
for(i = 0; i < X->row; i++){
if(y->data[i] == k){
for(l = 0; l < X->col; l++){
classes->m[k]->data[cc][l] = X->data[i][l];
}
lda->classid->data[j] = i;
j++;
cc++;
}
else{
continue;
}
}
}
}
else{
for(k = 0; k < lda->nclass; k++){
cc = 0;
for(i = 0; i < X->row; i++){
if(y->data[i] == k+1)
cc++;
else
continue;
}
NewArrayMatrix(&classes, k, cc, X->col);
cc = 0;
for(i = 0; i < X->row; i++){
if(y->data[i] == k+1){
for(l = 0; l < X->col; l++){
classes->m[k]->data[cc][l] = X->data[i][l];
}
lda->classid->data[j] = i;
j++;
cc++;
}
else{
continue;
}
}
}
}
/*puts("Classes"); PrintArray(classes);*/
/* Compute the prior probability */
for(k = 0; k < classes->order; k++){
DVectorAppend(&lda->pprob, (classes->m[k]->row/(double)X->row));
}
/*
puts("Prior Probability");
PrintDVector(lda->pprob);
*/
/*Compute the mean of each class*/
for(k = 0; k < classes->order; k++){
initDVector(&classmu);
MatrixColAverage(classes->m[k], &classmu);
MatrixAppendRow(&lda->mu, classmu);
DelDVector(&classmu);
}
/*puts("Class Mu");
FindNan(lda->mu);
PrintMatrix(lda->mu);*/
/*Calculate the total mean of samples..*/
initDVector(&mutot);
MatrixColAverage(X, &mutot);
/*puts("Mu tot");
PrintDVector(mutot);*/
/*NewDVector(&mutot, mu->col);
for(k = 0; k < mu->row; k++){
for(i = 0; i < mu->col; i++){
mutot->data[i] += mu->data[k][i];
}
}
for(i = 0; i < mutot->size; i++){
mutot->data[i] /= nclasses;
}*/
/*Centering data before computing the scatter matrix*/
for(k = 0; k < classes->order; k++){
for(i = 0; i < classes->m[k]->row; i++){
for(j = 0; j < classes->m[k]->col; j++){
classes->m[k]->data[i][j] -= mutot->data[j];
}
}
}
/*
puts("Classes");
for(i = 0; i < classes->order; i++){
FindNan(classes->m[i]);
}
PrintArray(classes);
*/
/*Compute the scatter matrix
* S = nobj - 1 * covmx
*/
initArray(&S);
NewMatrix(&covmx, X->col, X->col);
for(k = 0; k < classes->order; k++){
matrix *m_T;
NewMatrix(&m_T, classes->m[k]->col, classes->m[k]->row);
MatrixTranspose(classes->m[k], m_T);
MatrixDotProduct(m_T, classes->m[k], covmx);
for(i = 0; i < covmx->row; i++){
for(j = 0; j < covmx->col; j++){
covmx->data[i][j] /= classes->m[k]->row;
}
}
ArrayAppendMatrix(&S, covmx);
MatrixSet(covmx, 0.f);
DelMatrix(&m_T);
}
/*
puts("Scatter Matrix");
for(i = 0; i < S->order; i++)
FindNan(S->m[i]);
PrintArray(S);*/
/* Compute the class scatter which represent the covariance matrix */
NewMatrix(&Sw, X->col, X->col);
for(k = 0; k < S->order; k++){
for(i = 0; i < S->m[k]->row; i++){
for(j = 0; j < S->m[k]->col; j++){
Sw->data[i][j] += (double)(classes->m[k]->row/(double)X->row) * S->m[k]->data[i][j];
}
}
}
/*
puts("Class scatter matrix Sw");
FindNan(Sw);
PrintMatrix(Sw);
*/
/*Compute the between class scatter matrix Sb*/
NewMatrix(&Sb, X->col, X->col);
for(k = 0; k < lda->mu->row; k++){ /*for each class of object*/
cc = classes->m[k]->row;
for(i = 0; i < Sb->row; i++){
for(j = 0; j < Sb->col; j++){
Sb->data[i][j] += cc * (lda->mu->data[k][i] - mutot->data[i]) * (lda->mu->data[k][j] - mutot->data[j]);
}
}
}
/*
puts("Between class scatter matrix Sb");
FindNan(Sb);
PrintMatrix(Sb); */
/* Computing the LDA projection */
/*puts("Compute Matrix Inversion");*/
MatrixInversion(Sw, &lda->inv_cov);
double ss = 0.f;
for(i = 0; i < lda->inv_cov->row; i++){
for(j = 0; j < lda->inv_cov->col; j++){
ss += square(lda->inv_cov->data[i][j]);
}
if(_isnan_(ss))
break;
}
if(FLOAT_EQ(ss, 0.f, EPSILON) || _isnan_(ss)){
/*Do SVD as pseudoinversion accordin to Liu et al. because matrix is nonsingular
*
* JUN LIU et al, Int. J. Patt. Recogn. Artif. Intell. 21, 1265 (2007). DOI: 10.1142/S0218001407005946
* EFFICIENT PSEUDOINVERSE LINEAR DISCRIMINANT ANALYSIS AND ITS NONLINEAR FORM FOR FACE RECOGNITION
*
*
* Sw`^-1 = Q * G^-1 * Q_T
* Q G AND Q_T come from SVD
*/
MatrixPseudoinversion(Sw, &lda->inv_cov);
/*
NewMatrix(&A_T, Sw->col, Sw->row);
MatrixInversion(Sw, &A_T);
NewMatrix(&A_T_Sw, A_T->row, Sw->col);
MatrixDotProduct(A_T, Sw, A_T_Sw);
initMatrix(&A_T_Sw_inv);
MatrixInversion(A_T_Sw, &A_T_Sw_inv);
MatrixDotProduct(A_T_Sw_inv, A_T, lda->inv_cov);
DelMatrix(&A_T);
DelMatrix(&A_T_Sw);
DelMatrix(&A_T_Sw_inv);
*/
}
/*puts("Inverted Covariance Matrix from Sw");
FindNan(lda->inv_cov);
PrintMatrix(lda->inv_cov);
*/
/*puts("Compute Matrix Dot Product");*/
NewMatrix(&InvSw_Sb, lda->inv_cov->row, Sb->col);
MatrixDotProduct(lda->inv_cov, Sb, InvSw_Sb);
/*puts("InvSw_Sb"); PrintMatrix(InvSw_Sb);*/
/*puts("Compute Eigenvectors");*/
EVectEval(InvSw_Sb, &lda->eval, &lda->evect);
/*EvectEval3(InvSw_Sb, InvSw_Sb->row, &lda->eval, &lda->evect);*/
/*EVectEval(InvSw_Sb, &lda->eval, &lda->evect); */
/* Calculate the new projection in the feature space
*
* and the multivariate normal distribution
*/
/* Remove centering data */
for(k = 0; k < classes->order; k++){
for(i = 0; i < classes->m[k]->row; i++){
for(j = 0; j < classes->m[k]->col; j++){
classes->m[k]->data[i][j] += mutot->data[j];
}
}
}
initMatrix(&X_T);
for(k = 0; k < classes->order; k++){
/*printf("row %d col %d\n", (int)classes->m[k]->row, (int)classes->m[k]->col);*/
AddArrayMatrix(&lda->features, classes->m[k]->row, classes->m[k]->col);
AddArrayMatrix(&lda->mnpdf, classes->m[k]->row, classes->m[k]->col);
}
NewDVector(&evect_, lda->evect->row);
initDVector(&ldfeature);
ResizeMatrix(&lda->fmean, classes->order, lda->evect->col);
ResizeMatrix(&lda->fsdev, classes->order, lda->evect->col);
for(l = 0; l < lda->evect->col; l++){
for(i = 0; i < lda->evect->row; i++){
evect_->data[i] = lda->evect->data[i][l];
}
for(k = 0; k < classes->order; k++){
ResizeMatrix(&X_T, classes->m[k]->col, classes->m[k]->row);
MatrixTranspose(classes->m[k], X_T);
DVectorResize(&ldfeature, classes->m[k]->row);
DVectorMatrixDotProduct(X_T, evect_, ldfeature);
for(i = 0; i < ldfeature->size; i++){
lda->features->m[k]->data[i][l] = ldfeature->data[i];
}
/* Calculate the multivariate normal distribution */
double mean = 0.f, sdev = 0.f;
DVectorMean(ldfeature, &mean);
DVectorSDEV(ldfeature, &sdev);
lda->fmean->data[k][l] = mean;
lda->fsdev->data[k][l] = sdev;
for(i = 0; i < ldfeature->size; i++){
lda->mnpdf->m[k]->data[i][l] = 1./sqrt(2 * pi* sdev) * exp(-square((ldfeature->data[i] - mean)/sdev)/2.f);
}
}
}
DelDVector(&evect_);
DelMatrix(&covmx);
DelDVector(&ldfeature);
DelDVector(&mutot);
DelMatrix(&Sb);
DelMatrix(&InvSw_Sb);
DelArray(&classes);
DelArray(&S);
DelMatrix(&Sw);
DelMatrix(&X_T);
DelMatrix(&X);
}
