inline void RotateAndMove(float dX, float dY, float dZ, float rotation, float& x, float& y, float& z) {
if (rotation != 0) {
Rotate2D(rotation, x, z);
}
x += dX;
y += dY;
z += dZ;
}
void Projectile::TurnToTarget( float _dt )
{
// Calculate the vector from this token to the Target's position
D3DXVECTOR2 vToTarget(0,0);
D3DXVECTOR2 tDefault(0,-1);
D3DXVECTOR2 pos = ((BaseEntity*)m_Target)->GetPos() + ((BaseEntity*)m_Target)->GetImgCenter();
vToTarget.x = pos.x - (this->GetPos().x + this->GetImgCenter().x);
vToTarget.y = pos.y - (this->GetPos().y + this->GetImgCenter().y);
// Calculate forward vector
D3DXVECTOR2 forward = Rotate2D( tDefault, this->GetRot() );
// calculate the angle between the vectors
float angle = AngleBetweenVectors( vToTarget, forward );
if(Steering(forward, vToTarget) < 0.0f)
this->SetRot(this->GetRot() - 3.0f*_dt);
else
this->SetRot(this->GetRot() + 3.0f*_dt);
forward = Rotate2D( tDefault, this->GetRot() );
D3DXVec2Normalize(&forward,&forward);
this->SetDir(forward);
}
bool PolygonOrientation2D::PolygonRoationExperiment(const double & angle,
double& mbr_area_after_rotation)
{
//reverse the angle, then apply the rotation
double reverse_angle = angle * (-1);
Point2DArray rotated_vetices;
std::vector<Point2D>::iterator itVetex = m_polygon_centralized.begin();
for( ; itVetex!=m_polygon_centralized.end();itVetex++)
{
Point2D newp= Rotate2D(*itVetex,reverse_angle);
rotated_vetices.push_back(newp);
}
Point2D lt,rb;
MBR2D(rotated_vetices,lt,rb);
mbr_area_after_rotation = (fabs(lt.X-rb.X)+m_extended_baseline)*fabs(lt.Y-rb.Y);
return true;
}
void EQS_UVS2D_AI::Region( ELEM* elem,
PROJECT* project,
double** estifm,
double* force )
{
// -------------------------------------------------------------------------------------
// initializations
SHAPE* lShape = elem->GetLShape();
SHAPE* qShape = elem->GetQShape();
int ngp = qShape->ngp; // number of GAUSS points
int nnd = qShape->nnd; // number of nodes in all
int ncn = lShape->nnd; // number of corner nodes
TYPE* type = TYPE::Getid( elem->type );
int eqidV = nnd;
int eqidS = 2 * nnd;
if( force )
{
for( int i=0; i<maxEleq; i++ ) force[i] = 0.0;
}
if( estifm )
{
for( int i=0; i<maxEleq; i++ )
{
for( int j=0; j<maxEleq; j++ )
{
estifm[i][j] = 0.0;
}
}
}
double gravity = project->g;
double area = 0.0;
// -------------------------------------------------------------------------------------
// compute coordinates relative to first node
double x[kMaxNodes2D], y[kMaxNodes2D];
x[0] = elem->nd[0]->x;
y[0] = elem->nd[0]->y;
for( int i=1; i<nnd; i++ )
{
x[i] = elem->nd[i]->x - x[0];
y[i] = elem->nd[i]->y - y[0];
}
x[0] = y[0] = 0.0;
// -------------------------------------------------------------------------------------
// use GAUSS point integration to solve momentum equations for x- and
// y-direction (U and V) and continuity equation (H)
for( int g=0; g<ngp; g++ ) // START of loop over all GAUSS point
{
// -----------------------------------------------------------------------------------
// form JACOBIAN transformation matrix with quadratic shape functions
double* dfdxPtr = qShape->dfdx[g];
double* dfdyPtr = qShape->dfdy[g];
double trafo[2][2];
double detj = qShape->jacobi2D( nnd, dfdxPtr, dfdyPtr, x, y, trafo );
double weight = detj * qShape->weight[g];
area += weight;
// -----------------------------------------------------------------------------------
// compute values of quadratic shape functions at GP g
double dndx[kMaxNodes2D], dndy[kMaxNodes2D];
double* n = qShape->f[g];
for( int j=0; j<nnd; j++ )
{
dndx[j] = trafo[0][0] * dfdxPtr[j] + trafo[0][1] * dfdyPtr[j];
dndy[j] = trafo[1][0] * dfdxPtr[j] + trafo[1][1] * dfdyPtr[j];
}
// ----------------------------------------------------------------------------------
// compute values of linear shape functions at GP g
dfdxPtr = lShape->dfdx[g];
dfdyPtr = lShape->dfdy[g];
double dmdx[kMaxNodes2D], dmdy[kMaxNodes2D];
double* m = lShape->f[g];
for( int j=0; j<ncn; j++ )
{
dmdx[j] = trafo[0][0] * dfdxPtr[j] + trafo[0][1] * dfdyPtr[j];
dmdy[j] = trafo[1][0] * dfdxPtr[j] + trafo[1][1] * dfdyPtr[j];
}
// ------------------------------------------------------------------------------------
// compute flow parameters and their derivatives at GP g
// horizontal velocities: U and V
// flow depth : H
// Source or Sink : SS
// bottom elevation : a
// eddy viscosity : vt, cf
// Reynolds stresses : uu, uv, and vv
// integrate H, a and cf with linear shape
double H = 0.0;
double dHdt = 0.0;
double dHdx = 0.0;
double dHdy = 0.0;
double dadx = 0.0;
double dady = 0.0;
double cf = 0.0;
double SS = 0.0;
for( int j=0; j<ncn; j++ )
{
NODE* node = elem->nd[j];
BCON* bcon = &node->bc;
double ndZ = node->z;
double ndH = node->v.S - ndZ;
if( ndH <= 0.0 ) ndH = project->hmin;
double ndSS;
// -----------------------------------------------------------------------------------
// Source or Sink
//
// ndSS = -Q * ncn / A;
//
// bcon->val->Q - Q
// bcon->val->A - area of connected elements (see BCONSET::InitBcon)
// ncn - number of corner nodes
//
if( isFS(bcon->kind, BCON::kSource) )
{
ndSS = -bcon->val->Q * ncn / bcon->val->A;
}
else
{
ndSS = 0.0;
}
// -----------------------------------------------------------------------------------
H += m[j] * ndH;
dHdx += dmdx[j] * ndH;
dHdy += dmdy[j] * ndH;
dadx += dmdx[j] * ndZ;
dady += dmdy[j] * ndZ;
dHdt += m[j] * node->v.dSdt;
cf += m[j] * node->cf;
SS += m[j] * ndSS; // Source or Sink
}
// if( H <= 0.0 ) H = project->hmin;
// integrate U, V, uu, uv and vv with quadratic shape
double U = 0.0;
double dUdt = 0.0;
double dUdx = 0.0;
double dUdy = 0.0;
double V = 0.0;
double dVdt = 0.0;
double dVdx = 0.0;
double dVdy = 0.0;
double uu = 0.0;
double uv = 0.0;
double vv = 0.0;
for( int j=0; j<nnd; j++ )
{
NODE* node = elem->nd[j];
double ndU = node->v.U;
double ndV = node->v.V;
U += n[j] * ndU;
dUdt += n[j] * node->v.dUdt;
dUdx += dndx[j] * ndU;
dUdy += dndy[j] * ndU;
V += n[j] * ndV;
dVdt += n[j] * node->v.dVdt;
dVdx += dndx[j] * ndV;
dVdy += dndy[j] * ndV;
uu += n[j] * node->uu;
uv += n[j] * node->uv;
vv += n[j] * node->vv;
}
// compute dispersion coefficients ---------------------------------------------------
double Dxx = 0.0;
double Dxy = 0.0;
double Dyy = 0.0;
if( project->actualDisp > 0 )
{
for( int j=0; j<nnd; j++ )
{
NODE* node = elem->nd[j];
Dxx += n[j] * node->Dxx;
Dxy += n[j] * node->Dxy;
Dyy += n[j] * node->Dyy;
}
}
// compute eddy viscosity according to ELDER's assumption ----------------------------
double vtxx = 0.0;
double vtxy = 0.0;
double vtyy = 0.0;
for( int j=0; j<nnd; j++ )
{
NODE* node = elem->nd[j];
vtxx += n[j] * node->exx * node->vt;
vtxy += n[j] * node->exy * node->vt;
vtyy += n[j] * node->eyy * node->vt;
}
if( isFS(project->actualTurb, BCONSET::kVtMin) )
{
if( vtxx < type->vt ) vtxx = type->vt;
if( vtyy < type->vt ) vtyy = type->vt;
}
// add eddy viscosity to kinematic viscosity -----------------------------------------
vtxx += project->vk;
vtxy += project->vk;
vtyy += project->vk;
// -----------------------------------------------------------------------------------
// compute UVH-equation and coefficients of NEWTON-RAPHSON matrix
double Ures = sqrt( U*U + V*V ); // absolute velocity
if( force )
{
double f, fx, fy;
double* forcePtr;
// ---------------------------------------------------------------------------------
// compute x-momentum equation
f = H * dUdt; // time
f += H * (U * dUdx + V * dUdy); // convection
fx = H * (vtxx * dUdx + vtxy * dUdy); // eddy viscosity (approximation)
fy = H * (vtxy * dUdx + vtyy * dUdy);
fx -= H * uu; // turbulence
fy -= H * uv;
f += H * gravity * dadx; // gravity
fx -= H * H * gravity / 2.0;
f += cf * Ures * U; // bottom friction
// ------------------------------------------------ dispersion
// fx += H * Duu;
// fy += H * Duv;
fx += H * ( U*U*Dxx - 2.0*U*V*Dxy + V*V*Dyy );
fy += H * ( U*V*(Dxx-Dyy) + (U*U-V*V)*Dxy );
f *= weight;
fx *= weight;
fy *= weight;
forcePtr = force;
for( int j=0; j<nnd; j++ )
{
forcePtr[j] -= n[j] * f + dndx[j] * fx + dndy[j] * fy;
}
// ---------------------------------------------------------------------------------
// compute y-momentum equation
f = H * dVdt; // time
f += H * (U * dVdx + V * dVdy); // convection
fx = H * (vtxx * dVdx + vtxy * dVdy); // eddy viscosity (approximation)
fy = H * (vtxy * dVdx + vtyy * dVdy);
fx -= H * uv; // turbulence
fy -= H * vv;
f += H * gravity * dady; // gravity
fy -= H * H * gravity / 2.0;
f += cf * Ures * V; // bottom friction
// ------------------------------------------------ dispersion
// fx += H * Duv;
// fy += H * Dvv;
fx += H * ( U*V*(Dxx-Dyy) + (U*U-V*V)*Dxy );
fy += H * ( V*V*Dxx + 2.0*U*V*Dxy + U*U*Dyy );
f *= weight;
fx *= weight;
fy *= weight;
forcePtr = force + eqidV;
for( int j=0; j<nnd; j++ )
{
forcePtr[j] -= n[j] * f + dndx[j] * fx + dndy[j] * fy;
}
// ---------------------------------------------------------------------------------
// compute continuity equation (solve only for corner nodes)
f = dHdt + H * (dUdx + dVdy) + U * dHdx + V * dHdy;
f += SS; // Source or Sink
f *= weight;
forcePtr = force + eqidS;
for( int j=0; j<ncn; j++ )
{
forcePtr[j] -= m[j] * f;
}
}
if( estifm )
{
double t[kMaxNodes2D], tx[kMaxNodes2D], ty[kMaxNodes2D];
double df__, df_x, df_y, dfx_, dfxx, dfxy, dfy_, dfyx, dfyy;
double* estifmPtr;
// ---------------------------------------------------------------------------------
// compute components of NEWTON-RAPHSON Jacobi matrix
double iUres;
if( Ures > 1.0e-8 ) iUres = 1.0 / Ures;
else iUres = 0.0;
// U-derivative of x-momentum ------------------------------------------------------
df__ = weight * H * relaxThdt_UV;
df__ += weight * H * dUdx;
df_x = weight * H * U;
df_y = weight * H * V;
dfxx = weight * H * vtxx;
dfxy = weight * H * vtxy;
dfyx = weight * H * vtxy;
dfyy = weight * H * vtyy;
df__ += weight * cf * (iUres * U*U + Ures);
dfx_ = weight * H * ( 2.0*U*Dxx - 2.0*V*Dxy );
dfy_ = weight * H * ( V*(Dxx-Dyy) + 2.0*U*Dxy );
for( int j=0; j<nnd; j++ )
{
t[j] = df__ * n[j] + df_x * dndx[j] + df_y * dndy[j];
tx[j] = dfx_ * n[j] + dfxx * dndx[j] + dfxy * dndy[j];
ty[j] = dfy_ * n[j] + dfyx * dndx[j] + dfyy * dndy[j];
}
for( int j=0; j<nnd; j++ )
{
estifmPtr = estifm[j];
for( int k=0; k<nnd; k++ )
{
estifmPtr[k] += n[j]*t[k] + dndx[j]*tx[k] + dndy[j]*ty[k];
}
}
// V-derivative of x-momentum ------------------------------------------------------
df__ = weight * H * dUdy;
df__ += weight * cf * iUres * U * V;
dfx_ = weight * H * ( 2.0*V*Dyy - 2.0*V*Dxy );
dfy_ = weight * H * ( V*(Dxx-Dyy) - 2.0*V*Dxy );
for( int j=0; j<nnd; j++ )
{
t[j] = df__ * n[j];
tx[j] = dfx_ * n[j];
ty[j] = dfy_ * n[j];
}
for( int j=0; j<nnd; j++ )
{
estifmPtr = estifm[j] + eqidV;
for( int k=0; k<nnd; k++ )
{
estifmPtr[k] += n[j]*t[k] + dndx[j]*tx[k] + dndy[j]*ty[k];
}
}
// H-derivative of x-momentum ------------------------------------------------------
df__ = weight * dUdt;
df__ += weight * (U * dUdx + V * dUdy);
dfx_ = weight * (vtxx * dUdx + vtxy * dUdy);
dfy_ = weight * (vtxy * dUdx + vtyy * dUdy);
dfx_ -= weight * uu;
dfy_ -= weight * uv;
df__ += weight * gravity * dadx;
dfx_ -= weight * gravity * H;
dfx_ += weight * ( U*U*Dxx - 2.0*U*V*Dxy + V*V*Dyy );
dfy_ += weight * ( U*V*(Dxx-Dyy) + (U*U-V*V)*Dxy );
for( int j=0; j<ncn; j++ )
{
t[j] = df__ * m[j];
tx[j] = dfx_ * m[j];
ty[j] = dfy_ * m[j];
}
for( int j=0; j<nnd; j++ )
{
estifmPtr = estifm[j] + eqidS;
for( int k=0; k<ncn; k++ )
{
estifmPtr[k] += n[j]*t[k] + dndx[j]*tx[k] + dndy[j]*ty[k];
}
}
// U-derivative of y-momentum ------------------------------------------------------
df__ = weight * H * dVdx;
df__ += weight * cf * iUres * U * V;
dfx_ = weight * H * ( V*(Dxx-Dyy) + 2.0*U*Dxy );
dfy_ = weight * H * ( 2.0*V*Dxy + 2.0*U*Dyy );
for( int j=0; j<nnd; j++ )
{
t[j] = df__ * n[j];
tx[j] = dfx_ * n[j];
ty[j] = dfy_ * n[j];
}
for( int j=0; j<nnd; j++ )
{
estifmPtr = estifm[j + eqidV];
for( int k=0; k<nnd; k++ )
{
estifmPtr[k] += n[j]*t[k] + dndx[j]*tx[k] + dndy[j]*ty[k];
}
}
// V-derivative of y-momentum ------------------------------------------------------
df__ = weight * H * relaxThdt_UV;
df__ += weight * H * dVdy;
df_x = weight * H * U;
df_y = weight * H * V;
dfxx = weight * H * vtxx;
dfxy = weight * H * vtxy;
dfyx = weight * H * vtxy;
dfyy = weight * H * vtyy;
df__ += weight * cf * (iUres * V*V + Ures);
dfx_ = weight * H * ( U*(Dxx-Dyy) - 2.0*V*Dxy );
dfy_ = weight * H * ( 2.0*V*Dxx + 2.0*U*Dxy );
for( int j=0; j<nnd; j++ )
{
t[j] = df__ * n[j] + df_x * dndx[j] + df_y * dndy[j];
tx[j] = dfx_ * n[j] + dfxx * dndx[j] + dfxy * dndy[j];
ty[j] = dfy_ * n[j] + dfyx * dndx[j] + dfyy * dndy[j];
}
for( int j=0; j<nnd; j++ )
{
estifmPtr = estifm[j + eqidV] + eqidV;
for( int k=0; k<nnd; k++ )
{
estifmPtr[k] += n[j]*t[k] + dndx[j]*tx[k] + dndy[j]*ty[k];
}
}
// H-derivative of y-momentum ------------------------------------------------------
df__ = weight * dVdt;
df__ += weight * (U * dVdx + V * dVdy);
dfx_ = weight * (vtxx * dVdx + vtxy * dVdy);
dfy_ = weight * (vtxy * dVdx + vtyy * dVdy);
dfx_ -= weight * uv;
dfy_ -= weight * vv;
df__ += weight * gravity * dady;
dfy_ -= weight * gravity * H;
dfx_ += weight * ( U*V*(Dxx-Dyy) + (U*U-V*V)*Dxy );
dfy_ += weight * ( V*V*Dxx + 2.0*U*V*Dxy + U*U*Dyy );
for( int j=0; j<ncn; j++ )
{
t[j] = df__ * m[j];
tx[j] = dfx_ * m[j];
ty[j] = dfy_ * m[j];
}
for( int j=0; j<nnd; j++ )
{
estifmPtr = estifm[j + eqidV] + eqidS;
for( int k=0; k<ncn; k++ )
{
estifmPtr[k] += n[j]*t[k] + dndx[j]*tx[k] + dndy[j]*ty[k];
}
}
// U-derivative of continuity ------------------------------------------------------
df__ = weight * dHdx;
df_x = weight * H;
for( int j=0; j<nnd; j++ )
{
t[j] = df__ * n[j] + df_x * dndx[j];
}
for( int j=0; j<ncn; j++ )
{
estifmPtr = estifm[j + eqidS];
for( int k=0; k<nnd; k++ )
{
estifmPtr[k] += m[j]*t[k];
}
}
// V-derivative of continuity ------------------------------------------------------
df__ = weight * dHdy;
df_y = weight * H;
for( int j=0; j<nnd; j++ )
{
t[j] = df__ * n[j] + df_y * dndy[j];
}
for( int j=0; j<ncn; j++ )
{
estifmPtr = estifm[j + eqidS] + eqidV;
for( int k=0; k<nnd; k++ )
{
estifmPtr[k] += m[j]*t[k];
}
}
// H-derivative of continuity ------------------------------------------------------
df__ = weight * relaxThdt_H;
df__ += weight * (dUdx + dVdy);
df_x = weight * U;
df_y = weight * V;
for( int j=0; j<ncn; j++ )
{
t[j] = df__ * m[j] + df_x * dmdx[j] + df_y * dmdy[j];
}
for( int j=0; j<ncn; j++ )
{
estifmPtr = estifm[j + eqidS] + eqidS;
for( int k=0; k<ncn; k++ )
{
estifmPtr[k] += m[j]*t[k];
}
}
}
} // END of loop over all GAUSS point
// apply transformation ----------------------------------------------------------------
Rotate2D( nnd, eqidV, elem->nd, estifm, force );
// -------------------------------------------------------------------------------------
// insert experimental upstream boundary forces at nodes with inflow
// boundary condition (qfix = constant):
// flow depth at midside nodes is computed from values at corner nodes
// the x-momentum equation is replaced by:
// f = area * (qfix - Un * H)
// where: Un, H are computed values
for( int i=0; i<ncn; i++ ) // corner nodes
{
NODE* node = elem->nd[i];
BCON* bcon = &node->bc;
if(isFS(bcon->kind,BCON::kInlet) )
{
// set equation row dfU to zero ----------------------------------------------------
if( estifm )
{
for( int j=0; j<maxEleq; j++ )
{
estifm[i][j] = 0.0;
}
}
// compute flow in normal direction at node i --------------------------------------
double Un = node->v.U * bcon->niox + node->v.V * bcon->nioy;
double H = node->v.S - node->z;
double specQ = bcon->val->U;
if( H <= 0.0 )
{
H = project->hmin;
specQ = 0.0;
}
// force vector --------------------------------------------------------------------
if( force ) force[i] = area * (specQ - Un*H);
// jacobi matrix -------------------------------------------------------------------
if( estifm )
{
estifm[i][i] = area * H;
estifm[i][i + eqidS] = area * Un;
}
}
}
for( int i=ncn; i<nnd; i++ ) // midside nodes
{
NODE* node = elem->nd[i];
BCON* bcon = &node->bc;
if( isFS(bcon->kind, BCON::kInlet) )
{
// get left and right corner node to midside node i --------------------------------
int l, r;
qShape->getCornerNodes( i, &l, &r );
NODE* lnode = elem->nd[l];
NODE* rnode = elem->nd[r];
int nofeqHl = eqidS + l; // dfH at left corner node
int nofeqHr = eqidS + r; // dfH at right corner node
// set equation row dfU to zero ----------------------------------------------------
if( estifm )
{
for( int j=0; j<maxEleq; j++ )
{
estifm[i][j] = 0.0;
}
}
// compute flow in normal direction at node i --------------------------------------
double Un = node->v.U * bcon->niox + node->v.V * bcon->nioy;
double Hl = lnode->v.S - lnode->z;
double Hr = rnode->v.S - rnode->z;
double H = ( Hl + Hr ) / 2.0;
double specQ = bcon->val->U;
if( H <= 0.0 )
{
H = project->hmin;
specQ = 0.0;
}
// force vector --------------------------------------------------------------------
if( force ) force[i] = area * (specQ - Un*H);
// jacobi matrix -------------------------------------------------------------------
if( estifm )
{
estifm[i][i] = area * H;
estifm[i][nofeqHl] = area * Un / 2.0;
estifm[i][nofeqHr] = area * Un / 2.0;
}
}
}
}
void EQS_PPE2D::Bound( ELEM* elem,
PROJECT* project,
double** estifm,
double* force )
{
int i, j;
double* estifmPtr;
SHAPE* lShape = elem->GetLShape();
SHAPE* qShape = elem->GetQShape();
int nnd = qShape->nnd;
int startY = nnd;
int startP = 2 * nnd;
if( force ) for( i=0; i<maxEleq; i++ ) force[i] = 0.0;
if( estifm )
{
for( i=0; i<maxEleq; i++ )
{
estifmPtr = estifm[i];
for( j=0; j<maxEleq; j++ ) estifmPtr[j] = 0.0;
}
}
if( !estifm ) return;
if( isFS(elem->flag, ELEM::kOutlet) ) return;
NODE *node[3];
node[0] = elem->nd[0]; /* corner nodes */
node[1] = elem->nd[1];
node[2] = elem->nd[2]; /* midside node */
// row index -----------------------------------------------------------------------------------
int r0X = 0; int r1X = 1; int r2X = 2;
int r0Y = startY; int r1Y = startY + 1; int r2Y = startY + 2;
// column index --------------------------------------------------------------------------------
int c0P = startP; int c1P = startP + 1;
for( j=0; j<qShape->ngp; j++ ) // loop on GAUSS points
{
double weight, nx, ny;
double* m = lShape->f[j]; // linear shape
double* n = qShape->f[j]; // quadratic shape
double* dn = qShape->dfdx[j];
// -------------------------------------------------------------------------------------------
// compute normal vector
// notice: the normal is not reduced to unit length
nx = dn[0]*node[0]->y + dn[1]*node[1]->y + dn[2]*node[2]->y;
ny = -dn[0]*node[0]->x - dn[1]*node[1]->x - dn[2]*node[2]->x;
// weight of Gauss point j -------------------------------------------------------------------
weight = qShape->weight[j];
// compute estifm ----------------------------------------------------------------------------
estifm[r0X][c0P] -= n[0] * weight * nx * m[0];
estifm[r0X][c1P] -= n[0] * weight * nx * m[1];
estifm[r1X][c0P] -= n[1] * weight * nx * m[0];
estifm[r1X][c1P] -= n[1] * weight * nx * m[1];
estifm[r2X][c0P] -= n[2] * weight * nx * m[0];
estifm[r2X][c1P] -= n[2] * weight * nx * m[1];
estifm[r0Y][c0P] -= n[0] * weight * ny * m[0];
estifm[r0Y][c1P] -= n[0] * weight * ny * m[1];
estifm[r1Y][c0P] -= n[1] * weight * ny * m[0];
estifm[r1Y][c1P] -= n[1] * weight * ny * m[1];
estifm[r2Y][c0P] -= n[2] * weight * ny * m[0];
estifm[r2Y][c1P] -= n[2] * weight * ny * m[1];
}
// apply transformation ------------------------------------------------------------------------
Rotate2D( nnd, elem->nd, 3, estifm, force );
}
void EQS_PPE2D::Region( ELEM* elem,
PROJECT* project,
double** estifm,
double* force )
{
int i, j, k;
double* forcePtr;
double* estifmPtr;
double f;
double weight, detj;
double wh, wdhdx, wdhdy;
double h, dhdt, dhdx, dhdy;
double u, dudx;
double v, dvdy;
double trafo[2][2];
double x[kMaxNodes2D], y[kMaxNodes2D];
double dmdx[kMaxNodes2D], dmdy[kMaxNodes2D],
dndx[kMaxNodes2D], dndy[kMaxNodes2D];
// ---------------------------------------------------------------------------------------------
// initializations
SHAPE* lShape = elem->GetLShape();
SHAPE* qShape = elem->GetQShape();
int ngp = qShape->ngp; // number of GAUSS points
int ncn = lShape->nnd; // number of corner nodes
int nnd = qShape->nnd; // number of all nodes
if( force ) for( i=0; i<maxEleq; i++ ) force[i] = 0.0;
if( estifm )
{
for( i=0; i<maxEleq; i++ )
{
estifmPtr = estifm[i];
for( j=0; j<maxEleq; j++ ) estifmPtr[j] = 0.0;
}
}
int startY = nnd;
int startP = 2 * nnd;
NODE** nd = elem->nd;
// ---------------------------------------------------------------------------------------------
// compute coordinates relative to first node
x[0] = nd[0]->x;
y[0] = nd[0]->y;
for( i=1; i<nnd; i++ )
{
x[i] = nd[i]->x - *x;
y[i] = nd[i]->y - *y;
}
x[0] = y[0] = 0.0;
// ---------------------------------------------------------------------------------------------
// GAUSS point integration loop
for( i=0; i<ngp; i++ )
{
// -------------------------------------------------------------------------------------------
// form JACOBIAN transformation matrix with shape functions
double* dfdxPtr = qShape->dfdx[i];
double* dfdyPtr = qShape->dfdy[i];
detj = qShape->jacobi2D( nnd, dfdxPtr, dfdyPtr, x, y, trafo );
weight = detj * qShape->weight[i];
// -------------------------------------------------------------------------------------------
// compute values of quadaratic shape functions at GP i
double* n = qShape->f[i];
for( j=0; j<nnd; j++ )
{
dndx[j] = trafo[0][0] * dfdxPtr[j] + trafo[0][1] * dfdyPtr[j];
dndy[j] = trafo[1][0] * dfdxPtr[j] + trafo[1][1] * dfdyPtr[j];
}
// -------------------------------------------------------------------------------------------
// compute values of linear shape functions at GP i
dfdxPtr = lShape->dfdx[i];
dfdyPtr = lShape->dfdy[i];
double* m = lShape->f[i];
for( j=0; j<ncn; j++ )
{
dmdx[j] = trafo[0][0] * dfdxPtr[j] + trafo[0][1] * dfdyPtr[j];
dmdy[j] = trafo[1][0] * dfdxPtr[j] + trafo[1][1] * dfdyPtr[j];
}
// -------------------------------------------------------------------------------------------
// compute linear parameters at GP i
h = dhdt = dhdx = dhdy = 0.0;
for( j=0; j<ncn; j++ )
{
double ndH;
ndH = nd[j]->v.S - nd[j]->z;
h += m[j] * ndH;
dhdx += dmdx[j] * ndH;
dhdy += dmdy[j] * ndH;
dhdt += m[j] * nd[j]->v.dSdt;
}
// -------------------------------------------------------------------------------------------
// compute right side
if( force )
{
// -----------------------------------------------------------------------------------------
// compute qadratic parameters at GP i
u = dudx = v = dvdy = 0.0;
// # double duhdx = 0.0;
// # double dvhdy = 0.0;
for( j=0; j<nnd; j++ )
{
// # ------------------------------------------------------------------------
// # the following version, the minimisation of U*H and V*H instead of
// # U and V, does not lead to acceptable results !!
// #
// # double H = nd[j]->v.S - nd[j]->z;
// # duhdx += dndx[j] * (H * nd[j]->v.U);
// # dvhdy += dndy[j] * (H * nd[j]->v.V);
u += n[j] * nd[j]->v.U;
dudx += dndx[j] * nd[j]->v.U;
v += n[j] * nd[j]->v.V;
dvdy += dndy[j] * nd[j]->v.V;
}
// 3. equation -----------------------------------------------------------------------------
f = weight * ( dhdt + h*(dudx+dvdy) + u*dhdx + v*dhdy );
// # f = weight * ( dhdt + duhdx + dvhdy );
forcePtr = force + startP;
for( j=0; j<ncn; j++ )
{
forcePtr[j] -= m[j] * f;
}
}
// -------------------------------------------------------------------------------------------
// compute coefficients of element stiffness matrix
if( estifm )
{
// 1. equation, x-derivative of phi --------------------------------------------------------
for( j=0; j<nnd; j++ )
{
estifmPtr = estifm[j];
for( k=0; k<nnd; k++ )
{
estifmPtr[k] += n[j] * weight * n[k];
}
estifmPtr = estifm[j] + startP;
for( k=0; k<ncn; k++ )
{
estifmPtr[k] += dndx[j] * weight * m[k];
}
}
// 2. equation, y-derivative of phi --------------------------------------------------------
for( j=0; j<nnd; j++ )
{
estifmPtr = estifm[j + startY] + startY;
for( k=0; k<nnd; k++ )
{
estifmPtr[k] += n[j] * weight * n[k];
}
estifmPtr = estifm[j + startY] + startP;
for( k=0; k<ncn; k++ )
{
estifmPtr[k] += dndy[j] * weight * m[k];
}
}
// 3. equation, phi ------------------------------------------------------------------------
wdhdx = weight * dhdx;
wdhdy = weight * dhdy;
wh = weight * h;
for( j=0; j<ncn; j++ )
{
// ----------------------------
estifmPtr = estifm[j + startP];
for( k=0; k<nnd; k++ )
{
estifmPtr[k] += m[j] * (wh * dndx[k] + wdhdx * n[k]);
// # estifmPtr[k] += weight * m[j] * dndx[k];
}
// -------------------------------------
estifmPtr = estifm[j + startP] + startY;
for( k=0; k<nnd; k++ )
{
estifmPtr[k] += m[j] * (wh * dndy[k] + wdhdy * n[k]);
// # estifmPtr[k] += weight * m[j] * dndy[k];
}
// -------------------------------------
//estifmPtr = estifm[j + startP] + startP;
//for( k=0; k<ncn; k++ )
//{
// estifmPtr[k] += m[j] * weight * diagWeightPPE;
//}
}
}
}
// apply transformation ------------------------------------------------------------------------
Rotate2D( nnd, nd, 3, estifm, force );
}
void SimulateParticleSpawn(PartSysEmitter* emitter, int particleIdx, float timeToSimulate) {
const auto& spec = emitter->GetSpec();
auto partSpawnTime = emitter->GetParticleSpawnTime(particleIdx);
auto& worldPosVar = emitter->GetWorldPosVar();
auto particleX = worldPosVar.x;
auto particleY = worldPosVar.y;
auto particleZ = worldPosVar.z;
auto emitOffsetX = GetParticleValue(emitter, particleIdx, emit_offset_X, partSpawnTime);
auto emitOffsetY = GetParticleValue(emitter, particleIdx, emit_offset_Y, partSpawnTime);
auto emitOffsetZ = GetParticleValue(emitter, particleIdx, emit_offset_Z, partSpawnTime);
if (spec->GetOffsetCoordSys() == PartSysCoordSys::Polar) {
auto coords = SphericalDegToCartesian(emitOffsetX, emitOffsetY, emitOffsetZ);
particleX += coords.x;
particleY += coords.y;
particleZ += coords.z;
} else {
particleX += emitOffsetX;
particleY += emitOffsetY;
particleZ += emitOffsetZ;
}
switch (spec->GetSpace()) {
case PartSysEmitterSpace::ObjectPos:
case PartSysEmitterSpace::ObjectYpr: {
if (spec->GetParticleSpace() != PartSysParticleSpace::SameAsEmitter) {
// TODO: Figure out this formula...
auto scale = 1.0f - emitter->GetParticleAge(particleIdx) / timeToSimulate;
auto prevObjPos = emitter->GetPrevObjPos();
auto objPos = emitter->GetObjPos();
auto dX = prevObjPos.x + (objPos.x - prevObjPos.x) * scale;
auto dY = prevObjPos.y + (objPos.y - prevObjPos.y) * scale;
auto dZ = prevObjPos.z + (objPos.z - prevObjPos.z) * scale;
auto rotation = 0.0f;
if (spec->GetSpace() == PartSysEmitterSpace::ObjectYpr) {
rotation = emitter->GetPrevObjRotation() + (emitter->GetObjRotation() - emitter->GetPrevObjRotation()) * scale;
}
RotateAndMove(dX, dY, dZ, rotation, particleX, particleY, particleZ);
}
}
break;
case PartSysEmitterSpace::Bones: {
auto scale = 1.0f - emitter->GetParticleAge(particleIdx) / timeToSimulate;
if (!emitter->GetBoneState()) {
break;
}
XMFLOAT3 bonePos;
if (emitter->GetBoneState()->GetRandomPos(scale, &bonePos)) {
particleX = bonePos.x;
particleY = bonePos.y;
particleZ = bonePos.z;
}
}
break;
case PartSysEmitterSpace::NodePos:
case PartSysEmitterSpace::NodeYpr: {
if (spec->GetParticleSpace() != PartSysParticleSpace::SameAsEmitter) {
auto scale = 1.0f - emitter->GetParticleAge(particleIdx) / timeToSimulate;
if (spec->GetSpace() == PartSysEmitterSpace::NodeYpr) {
auto external = IPartSysExternal::GetCurrent();
Matrix4x4 matrix;
XMStoreFloat4x4(&matrix, DirectX::XMMatrixIdentity());
external->GetBoneWorldMatrix(emitter->GetAttachedTo(), spec->GetNodeName(), matrix);
auto boneM = DirectX::XMLoadFloat4x4(&matrix);
auto ppos = DirectX::XMVectorSet(particleX, particleY, particleZ, 1);
auto newpos = DirectX::XMVector3TransformCoord(ppos, boneM);
particleX = DirectX::XMVectorGetX(newpos);
particleY = DirectX::XMVectorGetY(newpos);
particleZ = DirectX::XMVectorGetZ(newpos);
} else {
auto prevObjPos = emitter->GetPrevObjPos();
auto objPos = emitter->GetObjPos();
auto dX = prevObjPos.x + (objPos.x - prevObjPos.x) * scale;
auto dY = prevObjPos.y + (objPos.y - prevObjPos.y) * scale;
auto dZ = prevObjPos.z + (objPos.z - prevObjPos.z) * scale;
RotateAndMove(dX, dY, dZ, 0.0f, particleX, particleY, particleZ);
}
}
}
break;
default:
break;
}
auto& state = emitter->GetParticleState();
state.SetState(PSF_X, particleIdx, particleX);
state.SetState(PSF_Y, particleIdx, particleY);
state.SetState(PSF_Z, particleIdx, particleZ);
// Initialize particle color
SetParticleParam(emitter, particleIdx, emit_init_red, partSpawnTime, PSF_RED, 255.0f);
SetParticleParam(emitter, particleIdx, emit_init_green, partSpawnTime, PSF_GREEN, 255.0f);
SetParticleParam(emitter, particleIdx, emit_init_blue, partSpawnTime, PSF_BLUE, 255.0f);
SetParticleParam(emitter, particleIdx, emit_init_alpha, partSpawnTime, PSF_ALPHA, 255.0f);
// Initialize particle velocity
auto partVelX = GetParticleValue(emitter, particleIdx, emit_init_vel_X, partSpawnTime);
auto partVelY = GetParticleValue(emitter, particleIdx, emit_init_vel_Y, partSpawnTime);
auto partVelZ = GetParticleValue(emitter, particleIdx, emit_init_vel_Z, partSpawnTime);
if (spec->GetSpace() == PartSysEmitterSpace::ObjectYpr) {
if (spec->GetParticleSpace() != PartSysParticleSpace::SameAsEmitter) {
auto scale = 1.0f - emitter->GetParticleAge(particleIdx) / timeToSimulate;
auto rotation = 0.0f;
if (spec->GetSpace() == PartSysEmitterSpace::ObjectYpr)
rotation = (emitter->GetObjRotation() - emitter->GetPrevObjRotation()) * scale + emitter->GetPrevObjRotation();
// Rotate the velocity vector according to the current object rotation in the world
// TODO: Even for rotation == 0, this will flip the velocity vector
Rotate2D(rotation, partVelX, partVelZ);
}
} else if (spec->GetSpace() == PartSysEmitterSpace::NodeYpr) {
if (spec->GetParticleSpace() != PartSysParticleSpace::SameAsEmitter) {
auto external = IPartSysExternal::GetCurrent();
auto objId = emitter->GetAttachedTo();
Matrix4x4 boneMatrix;
XMStoreFloat4x4(&boneMatrix, DirectX::XMMatrixIdentity());
external->GetBoneWorldMatrix(objId, spec->GetNodeName(), boneMatrix);
// Construct a directional vector (not a positional one, w=0 here) for the velocity
auto dirVec = DirectX::XMVectorSet(partVelX, partVelY, partVelZ, 0);
auto transMat = DirectX::XMLoadFloat4x4(&boneMatrix);
dirVec = DirectX::XMVector3TransformNormal(dirVec, transMat);
partVelX = DirectX::XMVectorGetX(dirVec);
partVelY = DirectX::XMVectorGetY(dirVec);
partVelZ = DirectX::XMVectorGetZ(dirVec);
}
}
// Are particle coordinates defined as polar coordinates? Convert them to cartesian here
if (spec->GetParticleVelocityCoordSys() == PartSysCoordSys::Polar) {
auto cartesianVel = SphericalDegToCartesian(partVelX, partVelY, partVelZ);
partVelX = cartesianVel.x;
partVelY = cartesianVel.y;
partVelZ = cartesianVel.z;
}
state.SetState(PSF_VEL_X, particleIdx, partVelX);
state.SetState(PSF_VEL_Y, particleIdx, partVelY);
state.SetState(PSF_VEL_Z, particleIdx, partVelZ);
// I don't know why it's taken at lifetime 0.
// TODO: Figure out if this actually *never* changes?
auto posVarX = GetParticleValue(emitter, particleIdx, part_posVariation_X, 0);
auto posVarY = GetParticleValue(emitter, particleIdx, part_posVariation_Y, 0);
auto posVarZ = GetParticleValue(emitter, particleIdx, part_posVariation_Z, 0);
// For a polar system, convert these positions to cartesian to apply them to the
// rendering position
if (spec->GetParticlePosCoordSys() == PartSysCoordSys::Polar) {
state.SetState(PSF_POS_AZIMUTH, particleIdx, 0);
state.SetState(PSF_POS_INCLINATION, particleIdx, 0);
state.SetState(PSF_POS_RADIUS, particleIdx, 0);
// Convert to cartesian and add to the actual current particle position
// As usual, x, y, z here are (azimuth, inclination, radius)
auto cartesianPos = SphericalDegToCartesian(posVarX, posVarY, posVarZ);
posVarX = cartesianPos.x;
posVarY = cartesianPos.y;
posVarZ = cartesianPos.z;
}
// Apply the position variation to the initial position and store it
posVarX += particleX;
posVarY += particleY;
posVarZ += particleZ;
state.SetState(PSF_POS_VAR_X, particleIdx, posVarX);
state.SetState(PSF_POS_VAR_Y, particleIdx, posVarY);
state.SetState(PSF_POS_VAR_Z, particleIdx, posVarZ);
/*
The following code will apply particle movement after
spawning a particle retroactively. This should only happen
for high frequency particle systems that spawn multiple particles
per frame.
Also note how particle age instead of particle lifetime is used here
to access parameters of the emitter.
*/
auto partAge = emitter->GetParticleAge(particleIdx);
if (partAge != 0) {
auto partAgeSquared = partAge * partAge;
particleX += partVelX * partAge;
particleY += partVelY * partAge;
particleZ += partVelZ * partAge;
auto param = emitter->GetParamState(part_accel_X);
if (param) {
auto accelX = param->GetValue(emitter, particleIdx, partAge);
particleX += accelX * partAgeSquared * 0.5f;
partVelX += accelX * partAge;
}
param = emitter->GetParamState(part_accel_Y);
if (param) {
auto accelY = param->GetValue(emitter, particleIdx, partAge);
particleY += accelY * partAgeSquared * 0.5f;
partVelY += accelY * partAge;
}
param = emitter->GetParamState(part_accel_Z);
if (param) {
auto accelZ = param->GetValue(emitter, particleIdx, partAge);
particleZ += accelZ * partAgeSquared * 0.5f;
partVelZ += accelZ * partAge;
}
state.SetState(PSF_VEL_X, particleIdx, partVelX);
state.SetState(PSF_VEL_Y, particleIdx, partVelY);
state.SetState(PSF_VEL_Z, particleIdx, partVelZ);
param = emitter->GetParamState(part_velVariation_X);
if (param) {
particleX += param->GetValue(emitter, particleIdx, partAge) * partAge;
}
param = emitter->GetParamState(part_velVariation_Y);
if (param) {
particleY += param->GetValue(emitter, particleIdx, partAge) * partAge;
}
param = emitter->GetParamState(part_velVariation_Z);
if (param) {
particleZ += param->GetValue(emitter, particleIdx, partAge) * partAge;
}
state.SetState(PSF_X, particleIdx, particleX);
state.SetState(PSF_Y, particleIdx, particleY);
state.SetState(PSF_Z, particleIdx, particleZ);
}
// Simulate rotation for anything other than a point particle
if (spec->GetParticleType() != PartSysParticleType::Point) {
auto emitterAge = emitter->GetParticleSpawnTime(particleIdx);
auto rotation = emitter->GetParamValue(emit_yaw, particleIdx, emitterAge, 0.0f);
emitter->GetParticleState().SetState(PSF_ROTATION, particleIdx, rotation);
}
}
void Painter::Rotate(double a)
{
Transform(Rotate2D(a));
}
