// Compute angular acceleration in body frame
// Parameters:
// I: inertia matrix
void angular_acceleration(float32_t *omegad , float32_t *inputs, float32_t* omega, float32_t* I, float32_t L, float32_t b, float32_t k)
{
float32_t tau[3];
float32_t tmp_3x1[3], cross[3];
float32_t II[9];
torques(tau, inputs, L, b, k);
Matrix_Inv_3x3(II, I);
Matrix_3x3_Multiply_Vector_3x1(tmp_3x1, I, omega);
Vector_Cross(cross, omega, tmp_3x1);
Vector_Subtract(tmp_3x1, tau, cross);
Matrix_3x3_Multiply_Vector_3x1(omegad, II, tmp_3x1);
}
//---------------------------------------------------------------------------//
// CalculaTangentes
//
//---------------------------------------------------------------------------//
void CObjeto3D::CalculaTangentes()
{
TVector3 *pTan1 = NEW_ARRAY(TVector3, m_uNumVertices);
//TVector3 *pTan2 = pTan1 + m_uNumVertices;
memset(pTan1, 0, m_uNumVertices * sizeof(TVector3));
for (int i = 0; i < m_uNumFaces; i++)
{
int i1 = m_pFaces[i].i0;
int i2 = m_pFaces[i].i1;
int i3 = m_pFaces[i].i2;
const TVector3 &v1 = m_pVertices[i1];
const TVector3 &v2 = m_pVertices[i2];
const TVector3 &v3 = m_pVertices[i3];
const TVector2 &w1 = m_pCacheUV[i*3+0];
const TVector2 &w2 = m_pCacheUV[i*3+1];
const TVector2 &w3 = m_pCacheUV[i*3+2];
float x1 = v2.x - v1.x;
float x2 = v3.x - v1.x;
float y1 = v2.y - v1.y;
float y2 = v3.y - v1.y;
float z1 = v2.z - v1.z;
float z2 = v3.z - v1.z;
float s1 = w2.x - w1.x;
float s2 = w3.x - w1.x;
float t1 = w2.y - w1.y;
float t2 = w3.y - w1.y;
float r = 1.0f / (s1 * t2 - s2 * t1);
TVector3 sdir((t2 * x1 - t1 * x2) * r, (t2 * y1 - t1 * y2) * r, (t2 * z1 - t1 * z2) * r);
//TVector3 tdir((s1 * x2 - s2 * x1) * r, (s1 * y2 - s2 * y1) * r, (s1 * z2 - s2 * z1) * r);
pTan1[i1].x += sdir.x;
pTan1[i1].y += sdir.y;
pTan1[i1].z += sdir.z;
pTan1[i2].x += sdir.x;
pTan1[i2].y += sdir.y;
pTan1[i2].z += sdir.z;
pTan1[i3].x += sdir.x;
pTan1[i3].y += sdir.y;
pTan1[i3].z += sdir.z;
/*
pTan2[i1].x += tdir.x; pTan2[i1].y += tdir.y; pTan2[i1].z += tdir.z;
pTan2[i2].x += tdir.x; pTan2[i2].y += tdir.y; pTan2[i2].z += tdir.z;
pTan2[i3].x += tdir.x; pTan2[i3].y += tdir.y; pTan2[i3].z += tdir.z;
*/
}
for (int i = 0; i < m_uNumVertices; i++)
{
TVector3 n = m_pVNormales[i];
TVector3 t = pTan1[i];
TVector3 r;
float fDot = Vector_Dot(&n, &t);
r.x = t.x - n.x * fDot;
r.y = t.y - n.y * fDot;
r.z = t.z - n.z * fDot;
Vector_Unit (&m_pTangentes[i], &r);
Vector_Cross(&m_pBinormales[i], &n, &m_pTangentes[i]);
}
DISPOSE_ARRAY(pTan1);
}
Bool pointInElement( void* _context, Coord point, Element_DomainIndex dElementInd ) {
Snac_Context* context = (Snac_Context*)_context;
Mesh* mesh = context->mesh;
NodeLayout* nLayout = mesh->layout->nodeLayout;
unsigned nEltNodes;
Node_DomainIndex* eltNodes;
Coord crds[8];
double bc[4];
unsigned inds[4];
unsigned eltNode_i;
/* Extract the element's nodes, of which there should be eight. */
{
Element_GlobalIndex gEltInd;
nEltNodes = 8;
eltNodes = Memory_Alloc_Array( Node_DomainIndex, nEltNodes, "SnacRemesher" );
gEltInd = Mesh_ElementMapDomainToGlobal( mesh, dElementInd );
nLayout->buildElementNodes( nLayout, gEltInd, eltNodes );
}
/* Convert the node indices back to domain values. */
for( eltNode_i = 0; eltNode_i < nEltNodes; eltNode_i++ ) {
eltNodes[eltNode_i] = Mesh_NodeMapGlobalToDomain( mesh, eltNodes[eltNode_i] );
}
/* Copy coordinates. */
for( eltNode_i = 0; eltNode_i < nEltNodes; eltNode_i++ )
memcpy( crds[eltNode_i], mesh->nodeCoord[eltNodes[eltNode_i]], sizeof(Coord) );
if( _HexaEL_FindTetBarycenter( crds, point, bc, inds, INCLUSIVE_UPPER_BOUNDARY, NULL, 0 ) ) {
return True;
}
return False;
#if 0
unsigned plane_i;
const unsigned planeInds[6][3] = { { 0, 4, 1 }, { 3, 2, 7 }, { 0, 3, 4 },
{ 1, 5, 2 }, { 0, 1, 3 }, { 5, 4, 6 } };
/* As we are dealing with hexahedral meshes with Snac, there will be six planes to check. */
for( plane_i = 0; plane_i < 6; plane_i++ ) {
Coord norm;
double dist;
/* Build the plane normal. */
{
Node_DomainIndex inds[3];
Coord tmpA, tmpB;
inds[0] = eltNodes[planeInds[plane_i][0]];
inds[1] = eltNodes[planeInds[plane_i][1]];
inds[2] = eltNodes[planeInds[plane_i][2]];
Vector_Sub( tmpA, mesh->nodeCoord[inds[1]], mesh->nodeCoord[inds[0]] );
Vector_Sub( tmpB, mesh->nodeCoord[inds[2]], mesh->nodeCoord[inds[0]] );
Vector_Cross( norm, tmpA, tmpB );
Vector_Norm( norm, norm );
}
/* Calc distance. */
dist = Vector_Dot( norm, mesh->nodeCoord[planeInds[plane_i][0]] );
/* Check which side of the plane our new point is. */
if( Vector_Dot( norm, point ) <= dist ) {
/* It is inside, so we may continue. */
continue;
}
else {
/* It's outside, so this is the wrong element. */
break;
}
}
/* Free the element nodes array. */
FreeArray( eltNodes );
/* Return appropriately. */
return (plane_i < 6) ? False : True;
#endif
}
