void kalmannFilter(kalmannData *KD, double y[2]){
double temp1[2][2];
double temp2[2][2];
double temp3[2][2];
double tempVect1[2];
double tempVect2[2];
//calculate Xd
Matrix_mal_Vektor_2D(KD->A,KD->Xlast, KD->Xd);
//calculate Pd
double transA[2][2];
Matrixmultiplikation2D(KD->A,KD->Plast,temp1); // KEINEN RESULT POINTER???? FRAGEN!!
Transponiert2D(KD->A, transA);
Matrixmultiplikation2D(temp1,transA,temp2);
Matrixaddition2D(temp2, KD->Q, KD->Pd);
//Calculate K
//klammer berechnen
double transC[2][2];
Transponiert2D(KD->C, transC);
Matrixmultiplikation2D(KD->C,KD->Pd,temp1);
Matrixmultiplikation2D(temp1, transC, temp2);
Matrixaddition2D(temp2, KD->R, temp3);
double inverseKlammer[2][2];
Matrix_Inverse(temp3, inverseKlammer);
//rest
Matrixmultiplikation2D(KD->Pd,transC, temp1);
Matrixmultiplikation2D(temp1, inverseKlammer, KD->K);
//calculate X
Matrix_mal_Vektor_2D(KD->C,KD->Xd,tempVect1);
Skalar_mal_Vektor(-1,tempVect1);
Vektoraddition2D(y,tempVect1,tempVect2);
Matrix_mal_Vektor_2D(KD->K,tempVect2, tempVect1);
Vektoraddition2D(KD->Xd,tempVect1,KD->X);
//calculate P
//klammer
Matrixmultiplikation2D(KD->K, KD->C, temp1);
Skalar_mal_Matrix(-1,temp1);
Matrixaddition2D(KD->I,temp1, temp2);
Matrixmultiplikation2D(temp2, KD->Pd,KD->P);
//"Aus neu mach alt"
KD->Xlast[0] = KD->X[0];
KD->Xlast[1] = KD->X[1];
KD->Plast[0][0] = KD->P[0][0];
KD->Plast[0][1] = KD->P[0][1];
KD->Plast[1][0] = KD->P[1][0];
KD->Plast[1][1] = KD->P[1][1];
}
PlutoMatrix *pluto_matrix_inverse(PlutoMatrix *mat)
{
assert(mat->nrows == mat->ncols);
PlutoMatrix *inv;
int dim = mat->nrows;
Matrix *pinv = Matrix_Alloc(dim, dim);
Matrix *pmat = pluto_matrix_to_polylib(mat);
Matrix_Inverse(pmat, pinv);
inv = polylib_matrix_to_pluto(pinv);
Matrix_Free(pmat);
Matrix_Free(pinv);
return inv;
}
/*
* Return the Z-polyhderon 'Zpol' in canonical form: 'Result' (for the Z-poly-
* hedron in canonical form) and Basis 'Basis' (for the basis with respect to
* which 'Result' is in canonical form.
*/
void CanonicalForm(ZPolyhedron *Zpol,ZPolyhedron **Result,Matrix **Basis) {
Matrix *B1 = NULL, *B2=NULL, *T1 , *B2inv;
int i, l1, l2;
Value tmp;
Polyhedron *Image, *ImageP;
Matrix *H, *U, *temp, *Hprime, *Uprime, *T2;
#ifdef DOMDEBUG
FILE *fp;
fp = fopen("_debug", "a");
fprintf(fp,"\nEntered CANONICALFORM\n");
fclose(fp);
#endif
if(isEmptyZPolyhedron (Zpol)) {
Basis[0] = Identity(Zpol->Lat->NbRows);
Result[0] = ZDomain_Copy (Zpol);
return ;
}
value_init(tmp);
l1 = FindHermiteBasisofDomain(Zpol->P,&B1);
Image = DomainImage (Zpol->P,(Matrix *)Zpol->Lat,MAXNOOFRAYS);
l2 = FindHermiteBasisofDomain(Image,&B2);
if (l1 != l2)
fprintf(stderr,"In CNF : Something wrong with the Input Zpolyhedra \n");
B2inv = Matrix_Alloc(B2->NbRows, B2->NbColumns);
temp = Matrix_Copy(B2);
Matrix_Inverse(temp,B2inv);
Matrix_Free(temp);
temp = Matrix_Alloc(B2inv->NbRows,Zpol->Lat->NbColumns);
T1 = Matrix_Alloc(temp->NbRows,B1->NbColumns);
Matrix_Product(B2inv,(Matrix *)Zpol->Lat,temp);
Matrix_Product(temp,B1,T1);
Matrix_Free(temp);
T2 = ChangeLatticeDimension(T1,l1);
temp = ChangeLatticeDimension(T2,T2->NbRows+1);
/* Adding the affine part */
for(i = 0; i < l1; i ++)
value_assign(temp->p[i][temp->NbColumns-1],T1->p[i][T1->NbColumns-1]);
AffineHermite(temp,&H,&U);
Hprime = ChangeLatticeDimension(H,Zpol->Lat->NbRows);
/* Exchanging the Affine part */
for(i = 0; i < l1; i ++) {
value_assign(tmp,Hprime->p[i][Hprime->NbColumns-1]);
value_assign(Hprime->p[i][Hprime->NbColumns-1],Hprime->p[i][H->NbColumns-1]);
value_assign(Hprime->p[i][H->NbColumns-1],tmp);
}
Uprime = ChangeLatticeDimension(U,Zpol->Lat->NbRows);
/* Exchanging the Affine part */
for (i = 0;i < l1; i++) {
value_assign(tmp,Uprime->p[i][Uprime->NbColumns-1]);
value_assign(Uprime->p[i][Uprime->NbColumns-1],Uprime->p[i][U->NbColumns-1]);
value_assign(Uprime->p[i][U->NbColumns-1],tmp);
}
Polyhedron_Free (Image);
Matrix_Free (B2inv);
B2inv = Matrix_Alloc(B1->NbRows, B1->NbColumns);
Matrix_Inverse(B1,B2inv);
ImageP = DomainImage(Zpol->P, B2inv, MAXNOOFRAYS);
Matrix_Free(B2inv);
Image = DomainImage(ImageP, Uprime, MAXNOOFRAYS);
Domain_Free(ImageP);
Result[0] = ZPolyhedron_Alloc(Hprime, Image);
Basis[0] = Matrix_Copy(B2);
/* Free the variables */
Polyhedron_Free (Image);
Matrix_Free (B1);
Matrix_Free (B2);
Matrix_Free (temp);
Matrix_Free (T1);
Matrix_Free (T2);
Matrix_Free (H);
Matrix_Free (U);
Matrix_Free (Hprime);
Matrix_Free (Uprime);
value_clear(tmp);
return;
} /* CanonicalForm */
ERet DeferredRenderer::RenderScene( const SceneView& sceneView, const Clump& sceneData )
{
gfxMARKER(RenderScene);
if( sceneView.viewportWidth != m_viewportWidth || sceneView.viewportHeight != m_viewportHeight )
{
this->ResizeBuffers( sceneView.viewportWidth, sceneView.viewportHeight, false );
m_viewportWidth = sceneView.viewportWidth;
m_viewportHeight = sceneView.viewportHeight;
}
mxDO(BeginRender_GBuffer());
G_PerCamera cbPerView;
{
cbPerView.g_viewMatrix = sceneView.viewMatrix;
cbPerView.g_viewProjectionMatrix = sceneView.viewMatrix * sceneView.projectionMatrix;
cbPerView.g_inverseViewMatrix = Matrix_OrthoInverse( sceneView.viewMatrix );
cbPerView.g_projectionMatrix = sceneView.projectionMatrix;
cbPerView.g_inverseProjectionMatrix = ProjectionMatrix_Inverse( sceneView.projectionMatrix );
cbPerView.g_inverseViewProjectionMatrix = Matrix_Inverse( cbPerView.g_viewProjectionMatrix );
cbPerView.g_WorldSpaceCameraPos = sceneView.worldSpaceCameraPos;
float n = sceneView.nearClip;
float f = sceneView.farClip;
float x = 1 - f / n;
float y = f / n;
float z = x / f;
float w = y / f;
cbPerView.g_ZBufferParams = Float4_Set( x, y, z, w );
cbPerView.g_ZBufferParams2 = Float4_Set( n, f, 1.0f/n, 1.0f/f );
float H = sceneView.projectionMatrix[0][0];
float V = sceneView.projectionMatrix[2][1];
float A = sceneView.projectionMatrix[1][2];
float B = sceneView.projectionMatrix[3][2];
cbPerView.g_ProjParams = Float4_Set( H, V, A, B );
// x = 1/H
// y = 1/V
// z = 1/B
// w = -A/B
cbPerView.g_ProjParams2 = Float4_Set( 1/H, 1/V, 1/B, -A/B );
llgl::UpdateBuffer(llgl::GetMainContext(), m_hCBPerCamera, sizeof(cbPerView), &cbPerView);
}
// G-Buffer Stage: Render all solid objects to a very sparse G-Buffer
{
gfxMARKER(Fill_Geometry_Buffer);
mxDO(FxSlow_SetRenderState(m_hRenderContext, *m_rendererData, "Default"));
G_PerObject cbPerObject;
TObjectIterator< rxModel > modelIt( sceneData );
while( modelIt.IsValid() )
{
const rxModel& model = modelIt.Value();
const Float3x4* TRS = model.m_transform;
{
cbPerObject.g_worldMatrix = Float3x4_Unpack( *TRS );
cbPerObject.g_worldViewMatrix = Matrix_Multiply(cbPerObject.g_worldMatrix, sceneView.viewMatrix);
cbPerObject.g_worldViewProjectionMatrix = Matrix_Multiply(cbPerObject.g_worldMatrix, sceneView.viewProjectionMatrix);
llgl::UpdateBuffer(m_hRenderContext, m_hCBPerObject, sizeof(cbPerObject), &cbPerObject);
}
const rxMesh* mesh = model.m_mesh;
for( int iSubMesh = 0; iSubMesh < mesh->m_parts.Num(); iSubMesh++ )
{
const rxSubmesh& submesh = mesh->m_parts[iSubMesh];
const rxMaterial* material = model.m_batches[iSubMesh];
const FxShader* shader = material->m_shader;
if( shader->localCBs.Num() )
{
mxASSERT(shader->localCBs.Num()==1);
const ParameterBuffer& uniforms = material->m_uniforms;
const FxCBuffer& rCB = shader->localCBs[0];
llgl::UpdateBuffer(m_hRenderContext, rCB.handle, uniforms.GetDataSize(), uniforms.ToPtr() );
}
llgl::DrawCall batch;
batch.Clear();
SetGlobalUniformBuffers( &batch );
BindMaterial( material, &batch );
batch.inputLayout = Rendering::g_inputLayouts[VTX_Draw];
batch.topology = mesh->m_topology;
batch.VB[0] = mesh->m_vertexBuffer;
batch.IB = mesh->m_indexBuffer;
batch.b32bit = (mesh->m_indexStride == sizeof(UINT32));
batch.baseVertex = submesh.baseVertex;
batch.vertexCount = submesh.vertexCount;
batch.startIndex = submesh.startIndex;
batch.indexCount = submesh.indexCount;
llgl::Submit(m_hRenderContext, batch);
}
modelIt.MoveToNext();
}
}
EndRender_GBuffer();
// Deferred Lighting Stage: Accumulate all lights as a screen space operation
{
gfxMARKER(Deferred_Lighting);
llgl::ViewState viewState;
{
viewState.Reset();
viewState.colorTargets[0].SetDefault();
viewState.targetCount = 1;
viewState.flags = llgl::ClearColor;
}
llgl::SubmitView(m_hRenderContext, viewState);
{
gfxMARKER(Directional_Lights);
mxDO(FxSlow_SetRenderState(m_hRenderContext, *m_rendererData, "Deferred_Lighting"));
FxShader* shader;
mxDO(GetByName(*m_rendererData, "deferred_directional_light", shader));
mxDO2(FxSlow_SetResource(shader, "GBufferTexture0", llgl::AsResource(m_colorRT0->handle), Rendering::g_samplers[PointSampler]));
mxDO2(FxSlow_SetResource(shader, "GBufferTexture1", llgl::AsResource(m_colorRT1->handle), Rendering::g_samplers[PointSampler]));
mxDO2(FxSlow_SetResource(shader, "DepthTexture", llgl::AsResource(m_depthRT->handle), Rendering::g_samplers[PointSampler]));
TObjectIterator< rxGlobalLight > lightIt( sceneData );
while( lightIt.IsValid() )
{
rxGlobalLight& light = lightIt.Value();
//mxDO(FxSlow_Commit(m_hRenderContext,shader));
DirectionalLight lightData;
{
lightData.direction = Matrix_TransformNormal(sceneView.viewMatrix, light.m_direction);
lightData.color = light.m_color;
}
mxDO2(FxSlow_UpdateUBO(m_hRenderContext,shader,"DATA",&lightData,sizeof(lightData)));
DrawFullScreenTriangle(shader);
lightIt.MoveToNext();
}
}
{
gfxMARKER(Point_Lights);
mxDO(FxSlow_SetRenderState(m_hRenderContext, *m_rendererData, "Deferred_Lighting"));
FxShader* shader;
mxDO(GetAsset(shader,MakeAssetID("deferred_point_light.shader"),m_rendererData));
mxDO2(FxSlow_SetResource(shader, "GBufferTexture0", llgl::AsResource(m_colorRT0->handle), Rendering::g_samplers[PointSampler]));
mxDO2(FxSlow_SetResource(shader, "GBufferTexture1", llgl::AsResource(m_colorRT1->handle), Rendering::g_samplers[PointSampler]));
mxDO2(FxSlow_SetResource(shader, "DepthTexture", llgl::AsResource(m_depthRT->handle), Rendering::g_samplers[PointSampler]));
TObjectIterator< rxLocalLight > lightIt( sceneData );
while( lightIt.IsValid() )
{
rxLocalLight& light = lightIt.Value();
//mxDO(FxSlow_Commit(m_hRenderContext,shader));
PointLight lightData;
{
Float3 viewSpaceLightPosition = Matrix_TransformPoint(sceneView.viewMatrix, light.position);
lightData.Position_InverseRadius = Float4_Set(viewSpaceLightPosition, 1.0f/light.radius);
lightData.Color_Radius = Float4_Set(light.color, light.radius);
}
mxDO2(FxSlow_UpdateUBO(m_hRenderContext,shader,"DATA",&lightData,sizeof(lightData)));
DrawFullScreenTriangle(shader);
lightIt.MoveToNext();
}
}
}
// Thursday, March 26, 2015 Implementing Weighted, Blended Order-Independent Transparency
//http://casual-effects.blogspot.ru/2015/03/implemented-weighted-blended-order.html
// Forward+ notes
//http://bioglaze.blogspot.fi/2014/07/2048-point-lights-60-fps.html
// Material Stage: Render all solid objects again while combining the lighting
// from Stage 2 with certain material-properties (colors, reflections, glow, fog, etc.)
// to produce the final image
#if 0
{
llgl::ViewState viewState;
{
viewState.Reset();
viewState.colorTargets[0].SetDefault();
viewState.targetCount = 1;
}
llgl::SubmitView(m_hRenderContext, viewState);
}
{
mxDO(FxSlow_SetRenderState(m_hRenderContext, *m_rendererData, "NoCulling"));
FxShader* shader;
mxDO(GetByName(*m_rendererData, "full_screen_triangle", shader));
mxDO(FxSlow_SetResource(shader, "t_sourceTexture", llgl::AsResource(pColorRT1->handle), Rendering::g_samplers[PointSampler]));
mxDO(FxSlow_Commit(m_hRenderContext,shader));
DrawFullScreenTriangle(shader);
}
#endif
#if 0
{
llgl::ViewState viewState;
{
viewState.Reset();
viewState.colorTargets[0].SetDefault();
viewState.targetCount = 1;
}
llgl::SubmitView(m_hRenderContext, viewState);
}
{
FxShader* shader;
mxDO(FxSlow_SetRenderState(m_hRenderContext, *m_rendererData, "Default"));
{
mxDO(GetAsset(shader,MakeAssetID("debug_draw_colored.shader"),m_rendererData));
mxDO(FxSlow_SetUniform(shader,"g_color",RGBAf::GRAY.ToPtr()));
mxDO(FxSlow_Commit(m_hRenderContext,shader));
DBG_Draw_Models_With_Custom_Shader(sceneView, sceneData, *shader);
}
mxDO(FxSlow_SetRenderState(m_hRenderContext, *m_rendererData, "NoCulling"));
{
mxDO(GetAsset(shader,MakeAssetID("debug_draw_normals.shader"),m_rendererData));
float lineLength = 4.0f;
mxDO(FxSlow_SetUniform(shader,"g_lineLength",&lineLength));
mxDO(FxSlow_Commit(m_hRenderContext,shader));
DBG_Draw_Models_With_Custom_Shader(sceneView, sceneData, *shader, Topology::PointList);
}
}
#endif
return ALL_OK;
}
/* headingKalman
*
* Implements a 1-dimensional, 1st order Kalman Filter
*
* That is, it deals with heading and heading rate (h and h') but no other
* state variables. The state equations are:
*
* X = A X^
* h = h + h'dt --> | h | = | 1 dt | | h |
* h' = h' | h' | | 0 1 | | h' |
*
* Kalman Filtering is not that hard. If it's hard you haven't found the right
* teacher. Try taking CS373 from Udacity.com
*
* This notation is Octave (Matlab) syntax and is based on the Bishop-Welch
* paper and references the equation numbers in that paper.
* http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
*
* returns : current heading estimate
*/
float headingKalman(float dt, float Hgps, bool gps, float dHgyro, bool gyro)
{
A[1] = dt;
/* Initialize, first time thru
x = H*z0
*/
//fprintf(stdout, "gyro? %c gps? %c\n", (gyro)?'Y':'N', (gps)?'Y':'N');
// Depending on what sensor measurements we've gotten,
// switch between observer (H) matrices and measurement noise (R) matrices
// TODO 3 incorporate HDOP or sat count in R
if (gps) {
H[0] = 1.0;
z[0] = Hgps;
} else {
H[0] = 0;
z[0] = 0;
}
if (gyro) {
H[3] = 1.0;
z[1] = dHgyro;
} else {
H[3] = 0;
z[1] = 0;
}
//Matrix_print(2,2, A, "1. A");
//Matrix_print(2,2, P, " P");
//Matrix_print(2,1, x, " x");
//Matrix_print(2,1, K, " K");
//Matrix_print(2,2, H, "2. H");
//Matrix_print(2,1, z, " z");
/**********************************************************************
* Predict
%
* In this step we "move" our state estimate according to the equation
*
* x = A*x; // Eq 1.9
***********************************************************************/
float xp[2];
Matrix_Multiply(2,2,1, xp, A, x);
//Matrix_print(2,1, xp, "3. xp");
/**********************************************************************
* We also have to "move" our uncertainty and add noise. Whenever we move,
* we lose certainty because of system noise.
*
* P = A*P*A' + Q; // Eq 1.10
***********************************************************************/
float At[4];
Matrix_Transpose(2,2, At, A);
float AP[4];
Matrix_Multiply(2,2,2, AP, A, P);
float APAt[4];
Matrix_Multiply(2,2,2, APAt, AP, At);
Matrix_Add(2,2, P, APAt, Q);
//Matrix_print(2,2, P, "4. P");
/**********************************************************************
* Measurement aka Correct
* First, we have to figure out the Kalman Gain which is basically how
* much we trust the sensor measurement versus our prediction.
*
* K = P*H'*inv(H*P*H' + R); // Eq 1.11
***********************************************************************/
float Ht[4];
//Matrix_print(2,2, H, "5. H");
Matrix_Transpose(2,2, Ht, H);
//Matrix_print(2,2, Ht, "5. Ht");
float HP[2];
//Matrix_print(2,2, P, "5. P");
Matrix_Multiply(2,2,2, HP, H, P);
//Matrix_print(2,2, HP, "5. HP");
float HPHt[4];
Matrix_Multiply(2,2,2, HPHt, HP, Ht);
//Matrix_print(2,2, HPHt, "5. HPHt");
float HPHtR[4];
//Matrix_print(2,2, R, "5. R");
Matrix_Add(2,2, HPHtR, HPHt, R);
//Matrix_print(2,2, HPHtR, "5. HPHtR");
Matrix_Inverse(2, HPHtR);
//Matrix_print(2,2, HPHtR, "5. HPHtR");
float PHt[2];
//Matrix_print(2,2, P, "5. P");
//Matrix_print(2,2, Ht, "5. Ht");
Matrix_Multiply(2,2,2, PHt, P, Ht);
//Matrix_print(2,2, PHt, "5. PHt");
Matrix_Multiply(2,2,2, K, PHt, HPHtR);
//Matrix_print(2,2, K, "5. K");
/**********************************************************************
* Then we determine the discrepancy between prediction and measurement
* with the "Innovation" or Residual: z-H*x, multiply that by the
* Kalman gain to correct the estimate towards the prediction a little
* at a time.
*
* x = x + K*(z-H*x); // Eq 1.12
***********************************************************************/
float Hx[2];
Matrix_Multiply(2,2,1, Hx, H, xp);
//Matrix_print(2,2, H, "6. H");
//Matrix_print(2,1, x, "6. x");
//Matrix_print(2,1, Hx, "6. Hx");
float zHx[2];
Matrix_Subtract(2,1, zHx, z, Hx);
zHx[0] = clamp180(zHx[0]);
//Matrix_print(2,1, z, "6. z");
//Matrix_print(2,1, zHx, "6. zHx");
float KzHx[2];
Matrix_Multiply(2,2,1, KzHx, K, zHx);
//Matrix_print(2,2, K, "6. K");
//Matrix_print(2,1, KzHx, "6. KzHx");
Matrix_Add(2,1, x, xp, KzHx);
x[0] = clamp360(x[0]); // Clamp to 0-360 range
//Matrix_print(2,1, x, "6. x");
/**********************************************************************
* We also have to adjust the certainty. With a new measurement, the
* estimate certainty always increases.
*
* P = (I-K*H)*P; // Eq 1.13
***********************************************************************/
float KH[4];
//Matrix_print(2,2, K, "7. K");
Matrix_Multiply(2,2,2, KH, K, H);
//Matrix_print(2,2, KH, "7. KH");
float IKH[4];
Matrix_Subtract(2,2, IKH, I, KH);
//Matrix_print(2,2, IKH, "7. IKH");
float P2[4];
Matrix_Multiply(2,2,2, P2, IKH, P);
Matrix_Copy(2, 2, P, P2);
//Matrix_print(2,2, P, "7. P");
return x[0];
}
