bool inCircle(Point* a, Point* b, Point* c, Point* d) {
long a2 = a->getX()*a->getX() + a->getY()*a->getY();
long b2 = b->getX()*b->getX() + b->getY()*b->getY();
long c2 = c->getX()*c->getX() + c->getY()*c->getY();
long d2 = d->getX()*d->getX() + d->getY()*d->getY();
long det44 = 0;
det44 += d2 * det33(a->getX(), a->getY(), 1, b->getX(), b->getY(), 1, c->getX(), c->getY(), 1);
det44 -= d->getX() * det33(a2, a->getY(), 1, b2, b->getY(), 1, c2, c->getY(), 1);
det44 += d->getY() * det33(a2, a->getX(), 1, b2, b->getX(), 1, c2, c->getX(), 1);
det44 -= 1 * det33(a2, a->getX(), a->getY(), b2, b->getX(), b->getY(), c2, c->getX(), c->getY());
if(det44 < 0)
return true;
return false;
}
int
PFEMElement3D::update()
{
// get nodal coordinates
Matrix A(4,4);
for(int i=0; i<4; i++) {
const Vector& coord = nodes[2*i]->getCrds();
const Vector& disp = nodes[2*i]->getTrialDisp();
A(i,0) = 1.0;
for(int j=1; j<4; j++) {
A(i,j) = coord(j-1) + disp(j-1);
}
}
// get jacobi
Matrix coef(4,4), sub(3,3);
Vector jacobi(4);
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
// sub matrix
for(int k=0; k<4; k++) {
if(k==i) continue;
int k1 = k;
if(k>i) k1--;
for(int l=0; l<4; l++) {
if(l==j) continue;
int l1 = l;
if(l>j) l1--;
sub(k1,l1) = A(k,l);
}
}
// cofactor
int sign = 1;
if((i+j)%2 == 1) sign = -1;
coef(i,j) = sign*det33(sub);
jacobi(i) += coef(i,j) * A(i,j);
}
}
// opserr<<"J = "<<jacobi;
J = jacobi(0);
// if(fabs(J) <= 1e-6) {
// opserr<<"WARNING: zero Jacobian for element ";
// opserr<<J<<" --PFEMElement3D::update\n";
// //return -1;
// }
// get derivatives
for(int i=0; i<4; i++) {
dNdx[i] = coef(i,1)/J;
dNdy[i] = coef(i,2)/J;
dNdz[i] = coef(i,3)/J;
//opserr<<"dNdx = "<<dNdx[i]<<"\n";
}
return 0;
}
/*
* Compute inverse matrix of 3x3 matrix 'in'.
* Return true if successful.
*/
bool
invert33(const float in[3][3], float out[3][3])
{
float d;
float d_i;
d = det33(in);
if (d == 0)
return false;
d_i = 1.0 / d;
#define D( \
a_i, a_j, b_i, b_j, \
c_i, c_j, d_i, d_j \
) \
det22(in[a_i-1][a_j-1], in[b_i-1][b_j-1], \
in[c_i-1][c_j-1], in[d_i-1][d_j-1])
float a[3][3] = {
/* row 0 */
{
D(2,2, 2,3,
3,2, 3,3),
D(1,3, 1,2,
3,3, 3,2),
D(1,2, 1,3,
2,2, 2,3)
},
/* row 1 */
{
D(2,3, 2,1,
3,3, 3,1),
D(1,1, 1,3,
3,1, 3,3),
D(1,3, 1,1,
2,3, 2,1)
},
/* row 2 */
{
D(2,1, 2,2,
3,1, 3,2),
D(1,2, 1,1,
3,2, 3,1),
D(1,1, 1,2,
2,1, 2,2)
},
};
#undef D
int i, j;
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
out[i][j] = d_i * a[i][j];
return true;
}
void VSObjectTransformation::applyToKspace(double &kx,double &ky,double &kz,double &phase_offset,double &scale_factor) const {
scale_factor=det33(m_data);
phase_offset=(m_data[0][3]*kx+m_data[1][3]*ky+m_data[2][3]*kz)*360; //perhaps multiply by -1?
//multiply by transpose
double kx2=kx; double ky2=ky; double kz2=kz;
kx=m_data[0][0]*kx2+m_data[1][0]*ky2+m_data[2][0]*kz2;
ky=m_data[0][1]*kx2+m_data[1][1]*ky2+m_data[2][1]*kz2;
kz=m_data[0][2]*kx2+m_data[1][2]*ky2+m_data[2][2]*kz2;
}
// обращение матрицы 3*3
MATR33 ObrMatr(const MATR33 &m)
{ MATR33 mo; double det=det33(m);
mo.m11= (m.m22*m.m33-m.m23*m.m32)/det;
mo.m12=-(m.m21*m.m33-m.m23*m.m31)/det;
mo.m13= (m.m21*m.m32-m.m22*m.m31)/det;
mo.m21=-(m.m12*m.m33-m.m13*m.m32)/det;
mo.m22= (m.m11*m.m33-m.m13*m.m31)/det;
mo.m23=-(m.m11*m.m32-m.m12*m.m31)/det;
mo.m31= (m.m12*m.m23-m.m13*m.m22)/det;
mo.m32=-(m.m11*m.m23-m.m13*m.m21)/det;
mo.m33= (m.m11*m.m22-m.m12*m.m21)/det;
return mo;
}
void VSObjectTransformation::invert() {
double44 result;
set_identity(result);
double d33=det33(m_data);
result[0][0]=(m_data[1][1]*m_data[2][2]-m_data[2][1]*m_data[1][2])*(+1)/d33;
result[1][0]=(m_data[1][0]*m_data[2][2]-m_data[2][0]*m_data[1][2])*(-1)/d33;
result[2][0]=(m_data[1][0]*m_data[2][1]-m_data[2][0]*m_data[1][1])*(+1)/d33;
result[0][1]=(m_data[0][1]*m_data[2][2]-m_data[2][1]*m_data[0][2])*(-1)/d33;
result[1][1]=(m_data[0][0]*m_data[2][2]-m_data[2][0]*m_data[0][2])*(+1)/d33;
result[2][1]=(m_data[0][0]*m_data[2][1]-m_data[2][0]*m_data[0][1])*(-1)/d33;
result[0][2]=(m_data[0][1]*m_data[1][2]-m_data[1][1]*m_data[0][2])*(+1)/d33;
result[1][2]=(m_data[0][0]*m_data[1][2]-m_data[1][0]*m_data[0][2])*(-1)/d33;
result[2][2]=(m_data[0][0]*m_data[1][1]-m_data[1][0]*m_data[0][1])*(+1)/d33;
result[0][3]=-result[0][0]*m_data[0][3]-result[0][1]*m_data[1][3]-result[0][2]*m_data[2][3];
result[1][3]=-result[1][0]*m_data[0][3]-result[1][1]*m_data[1][3]-result[1][2]*m_data[2][3];
result[2][3]=-result[2][0]*m_data[0][3]-result[2][1]*m_data[1][3]-result[2][2]*m_data[2][3];
set_data(result);
}
