3277 lines
102 KiB
C++
3277 lines
102 KiB
C++
//===================== Copyright (c) Valve Corporation. All Rights Reserved. ======================
|
|
#include "mathlib/femodelbuilder.h"
|
|
#include "tier1/utlhashtable.h"
|
|
#include "tier1/utlsortvector.h"
|
|
#include "tier1/heapsort.h"
|
|
#include "bitvec.h"
|
|
#include "tier1/utlpair.h"
|
|
#include "modellib/clothhelpers.h"
|
|
#include "tier1/utlstringtoken.h"
|
|
#include "mathlib/feagglomerator.h"
|
|
#include <algorithm>
|
|
#include "femodel.inl"
|
|
|
|
// this function is exclusively for use with invM0/(invM0+invM1) type of formulae
|
|
// if one of the particles is almost infinite mass, I don't want to see any creep in its position (mostly for debugging)
|
|
inline float SnapWeight01( float w )
|
|
{
|
|
Assert( w >= 0.0f && w <= 1.0f );
|
|
return w < 1e-6f ? 0 : w > 0.99999f ? 1.0f : w;
|
|
}
|
|
|
|
inline float SnapWeight0( float w )
|
|
{
|
|
Assert( w >= 0.0f && w <= 1.0f );
|
|
return w < 1e-12f ? 0 : w ;
|
|
}
|
|
|
|
inline bool Is0To1( float w )
|
|
{
|
|
return w >= 0 && w <= 1.0f;
|
|
}
|
|
|
|
|
|
inline void KeepIn01( float &ref )
|
|
{
|
|
if ( ref > 1.0f )
|
|
{
|
|
ref = 1.0f;
|
|
}
|
|
else if ( !( ref >= 0.0f ) ) // the "!" should catch inf or nan
|
|
{
|
|
ref = 0.0f;
|
|
}
|
|
}
|
|
|
|
void CFeModelBuilder::BuildKelagerBends( )
|
|
{
|
|
CUtlVector< CUtlVector< uint16 > * > neighbors;
|
|
neighbors.SetCount( m_Nodes.Count( ) );
|
|
for ( int i = 0; i < m_Nodes.Count( ); ++i )
|
|
{
|
|
( neighbors[ i ] = new CUtlVector< uint16 > )->EnsureCapacity( 8 );
|
|
}
|
|
CVarBitVec staticNodes( m_Nodes.Count( ) );
|
|
|
|
int nMaxValency = 0;
|
|
for ( int i = 0; i < m_Elems.Count( ); ++i )
|
|
{
|
|
BuildElem_t elem = m_Elems[ i ];
|
|
uint nElemEdges = elem.nNode[ 3 ] == elem.nNode[ 2 ] ? 3 : 4;
|
|
for ( uint j = 0; j < nElemEdges; ++j )
|
|
{
|
|
uint j1 = ( j + 1 ) % nElemEdges;
|
|
uint v0 = elem.nNode[ j ], v1 = elem.nNode[ j1 ];
|
|
if ( j < elem.nStaticNodes )
|
|
{
|
|
staticNodes.Set( v0 );
|
|
}
|
|
if ( neighbors[ v0 ]->Find( v1 ) < 0 )
|
|
{
|
|
int v0elem = neighbors[ v0 ]->AddToTail( v1 );
|
|
int v1elem = neighbors[ v1 ]->AddToTail( v0 );
|
|
nMaxValency = Max( nMaxValency, Max( v0elem, v1elem ) + 1 );
|
|
}
|
|
}
|
|
}
|
|
|
|
// now we know which vertex is connected to which, and which is static
|
|
CVarBitVec paired( nMaxValency );
|
|
|
|
for ( int nNode = 0; nNode < m_Nodes.Count( ); ++nNode )
|
|
{
|
|
const CUtlVector< uint16 > &ring = *( neighbors[ nNode ] );
|
|
paired.ClearAll( );
|
|
|
|
Vector v = m_Nodes[ nNode ].transform.m_vPosition;
|
|
|
|
for ( int b0idx = 0; b0idx < ring.Count( ); ++b0idx )
|
|
{
|
|
if ( paired[ b0idx ] )
|
|
continue;
|
|
float flBestCos = -0.87f, flBestH0 = 0;
|
|
int nBestB1idx = -1;
|
|
int b0 = ring[ b0idx ];
|
|
int isB0static = staticNodes[ b0 ];
|
|
Vector vB0 = m_Nodes[ b0 ].transform.m_vPosition - v;
|
|
for ( int b1idx = 0; b1idx < ring.Count( ); ++b1idx )
|
|
{
|
|
if ( b1idx == b0idx )
|
|
continue;
|
|
int b1 = ring[ b1idx ];
|
|
if ( isB0static && staticNodes[ b1 ] )
|
|
continue; // no need to check static-static
|
|
Vector vB1 = m_Nodes[ b1 ].transform.m_vPosition - v;
|
|
float flCos = DotProduct( vB1.Normalized( ), vB0.Normalized( ) );
|
|
if ( flCos < flBestCos )
|
|
{
|
|
flBestCos = flCos;
|
|
nBestB1idx = b1idx;
|
|
flBestH0 = ( vB1 + vB0 ).Length( ) / 3.0f;
|
|
}
|
|
}
|
|
|
|
if ( nBestB1idx >= 0 )
|
|
{
|
|
FeKelagerBend_t kbend;
|
|
kbend.flHeight0 = flBestH0;
|
|
kbend.m_nNode[ 0 ] = nNode;
|
|
kbend.m_nNode[ 1 ] = b0;
|
|
kbend.m_nNode[ 2 ] = ring[ nBestB1idx ];
|
|
kbend.m_nFlags = 7;
|
|
for ( int j = 0; j < 3; ++j )
|
|
{
|
|
if ( staticNodes[ kbend.m_nNode[ j ] ] )
|
|
kbend.m_nFlags ^= 1 << j;
|
|
}
|
|
Assert( kbend.m_nFlags );
|
|
m_KelagerBends.AddToTail( kbend );
|
|
paired.Set( nBestB1idx );
|
|
}
|
|
}
|
|
}
|
|
|
|
neighbors.PurgeAndDeleteElements( );
|
|
}
|
|
|
|
|
|
|
|
void CFeModelBuilder::BuildAndSortRods( float flCurvatureAngle, bool bTriangulate ) // flCurvatureAngle is additional angle we can bend each side
|
|
{
|
|
CUtlHashtable< uint32, uint32 > edgeToRod;
|
|
|
|
// TODO: filter out rods that connect invalid here?
|
|
|
|
if ( m_bAddStiffnessRods )
|
|
{
|
|
if ( bTriangulate )
|
|
{
|
|
for ( int nElem = 0; nElem < m_Elems.Count(); ++nElem )
|
|
{
|
|
const BuildElem_t &elem = m_Elems[ nElem ];
|
|
if ( elem.nNode[ 2 ] != elem.nNode[ 3 ] )
|
|
{
|
|
uint v0 = elem.nNode[ 1 ], v1 = elem.nNode[ 3 ];
|
|
BuildRod( flCurvatureAngle, v0, v1, nElem, nElem, elem.nNode[ 0 ], elem.nNode[ 2 ], edgeToRod );
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
for ( int nEdge = 0; nEdge < m_FeEdgeDesc.Count(); ++nEdge )
|
|
{
|
|
FeEdgeDesc_t edge = m_FeEdgeDesc[ nEdge ];
|
|
|
|
for ( uint nLong = 0; nLong < 2; ++nLong )
|
|
{
|
|
uint v0 = edge.nSide[ 0 ][ nLong ], v1 = edge.nSide[ 1 ][ nLong ];
|
|
BuildRod( flCurvatureAngle, v0, v1, edge.nElem[ 0 ], edge.nElem[ 1 ], edge.nEdge[ 0 ], edge.nEdge[ 1 ], edgeToRod );
|
|
}
|
|
}
|
|
}
|
|
for ( int i = 0; i < m_Rods.Count( ); ++i )
|
|
{
|
|
FeRodConstraint_t &rod = m_Rods[ i ];
|
|
if ( m_Nodes[ rod.nNode[ 0 ] ].nRank > m_Nodes[ rod.nNode[ 1 ] ].nRank )
|
|
{
|
|
rod.InvariantReverse( );
|
|
}
|
|
}
|
|
|
|
HeapSort( m_Rods, [&] ( const FeRodConstraint_t &left, const FeRodConstraint_t &right ) {
|
|
int nRankLeft = Min( m_Nodes[ left.nNode[ 0 ] ].nRank, m_Nodes[ left.nNode[ 1 ] ].nRank );
|
|
int nRankRight = Min( m_Nodes[ right.nNode[ 0 ] ].nRank, m_Nodes[ right.nNode[ 1 ] ].nRank );
|
|
if ( nRankLeft == nRankRight )
|
|
{
|
|
float flLeft = Min( left.flWeight0, 1 - left.flWeight0 );
|
|
float flRight = Min( right.flWeight0, 1 - right.flWeight0 );
|
|
return flLeft < flRight;
|
|
}
|
|
else
|
|
{
|
|
return nRankLeft < nRankRight;
|
|
}
|
|
} );
|
|
}
|
|
|
|
|
|
void CFeModelBuilder::BuildRod( float flCurvatureAngle, uint v0, uint v1, uint nElem0, uint nElem1, uint nEdgeV0, uint nEdgeV1, CUtlHashtable< uint32, uint32 > &edgeToRod )
|
|
{
|
|
if ( !m_Nodes[ v0 ].bSimulated && m_Nodes[ v1 ].bSimulated )
|
|
return; // rods between static nodes don't work
|
|
uint nRodV0 = v0 | ( v1 << 16 ), nRodV1 = v1 | ( v0 << 16 );
|
|
float invM0 = m_Nodes[ v0 ].invMass, invM1 = m_Nodes[ v1 ].invMass;
|
|
if ( invM1 + invM0 < 1e-12f )
|
|
return; // doesn't matter, this rod is between two infinitely massive nodes
|
|
|
|
Vector vEdge = ( m_Nodes[ nEdgeV1 ].transform.m_vPosition - m_Nodes[ nEdgeV0 ].transform.m_vPosition ).NormalizedSafe( Vector( 0, 0, 1 ) );
|
|
float flRelaxedRodLength = ( m_Nodes[ v1 ].transform.m_vPosition - m_Nodes[ v0 ].transform.m_vPosition ).Length();
|
|
float flSumSlack = m_Elems[ nElem0 ].flSlack + m_Elems[ nElem1 ].flSlack;
|
|
|
|
// max distance happens when the two polygons are rotated around edge.nEdge[] so that v0, v1 and edge.nEdge[] are in one plane
|
|
// then, the angle v0-edge.nEdge[1-nLong]-v1 is at its maximum, and is the sum of angles in each quad's plane
|
|
float flMaxDist, flMinDist;
|
|
if ( m_bRigidEdgeHinges )
|
|
{
|
|
flMaxDist = flMinDist = flRelaxedRodLength;
|
|
}
|
|
else
|
|
{
|
|
Vector vCenter = m_Nodes[ nEdgeV0 ].transform.m_vPosition;
|
|
Vector vSide[ 2 ] = { m_Nodes[ v0 ].transform.m_vPosition - vCenter, m_Nodes[ v1 ].transform.m_vPosition - vCenter };
|
|
float flSideProj[ 2 ] = { DotProduct( vSide[ 0 ], vEdge ), DotProduct( vSide[ 1 ], vEdge ) }; // projection of side vectors onto the hinge edge
|
|
float flSideProjDelta = flSideProj[ 0 ] - flSideProj[ 1 ];
|
|
Vector vSideHeight[ 2 ] = { vSide[ 0 ] - vEdge * flSideProj[ 0 ], vSide[ 1 ] - vEdge * flSideProj[ 1 ] };// orthogonal to the hinge components of the sides
|
|
float h[ 2 ] = { vSideHeight[ 0 ].Length( ), vSideHeight[ 1 ].Length( ) };
|
|
flMaxDist = sqrtf( Sqr( flSideProjDelta ) + Sqr( h[ 0 ] + h[ 1 ] ) ) + flSumSlack;
|
|
// min distance happens when triangles are closed like a book at angle flCurvatureAngle. THe points on triangles are running in circles, so it's a distance between points on 2 circles in 3D
|
|
Vector deltaMin( h[ 1 ] * cosf( flCurvatureAngle ) - h[ 0 ], h[1] * sinf( flCurvatureAngle ), flSideProjDelta );
|
|
flMinDist = Min( deltaMin.Length() - flSumSlack, flRelaxedRodLength ); // the mindist cannot be higher than relaxed pose mindist
|
|
}
|
|
Assert( flMinDist <= flMaxDist * 1.0001f + FLT_EPSILON );
|
|
UtlHashHandle_t hFind = edgeToRod.Find( nRodV0 );
|
|
uint nRodVidx = 0;
|
|
if ( hFind == edgeToRod.InvalidHandle( ) )
|
|
{
|
|
hFind = edgeToRod.Find( nRodV1 );
|
|
nRodVidx = 1;
|
|
}
|
|
if ( hFind == edgeToRod.InvalidHandle( ) )
|
|
{
|
|
nRodVidx = 0;
|
|
|
|
FeRodConstraint_t rod;
|
|
rod.nNode[ 0 ] = v0;
|
|
rod.nNode[ 1 ] = v1;
|
|
rod.flRelaxationFactor = 1.0f;
|
|
rod.flWeight0 = SnapWeight01( invM0 / ( invM1 + invM0 ) );
|
|
rod.flMinDist = flMinDist;
|
|
rod.flMaxDist = flMaxDist;
|
|
|
|
int nRod = m_Rods.AddToTail( rod );
|
|
edgeToRod.Insert( nRodV0, ( uint ) nRod );
|
|
}
|
|
else
|
|
{
|
|
FeRodConstraint_t &rod = m_Rods[ edgeToRod[ hFind ] ];
|
|
Assert( nRodVidx == 0 ? ( rod.nNode[ 0 ] == v0 && rod.nNode[ 1 ] == v1 ) : ( rod.nNode[ 0 ] == v1 && rod.nNode[ 1 ] == v0 ) );
|
|
rod.flMinDist = Min( rod.flMinDist, flMinDist );
|
|
rod.flMaxDist = Max( rod.flMaxDist, flMaxDist );
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void CFeModelBuilder::BuildSprings( CUtlVector< FeSpringIntegrator_t > &springs )
|
|
{
|
|
CUtlHashtable< uint32 > created;
|
|
|
|
springs.EnsureCapacity( m_Springs.Count() );
|
|
for ( int i = 0; i < m_Springs.Count( ); ++i )
|
|
{
|
|
FeSpringIntegrator_t spring;
|
|
spring.nNode[ 0 ] = m_Springs[ i ].nNode[ 0 ];
|
|
spring.nNode[ 1 ] = m_Springs[ i ].nNode[ 1 ];
|
|
|
|
if ( created.HasElement( spring.nNode[ 0 ] + ( uint( spring.nNode[ 1 ] ) << 16 ) ) )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
const BuildNode_t &node0 = m_Nodes[ spring.nNode[ 0 ] ], &node1 = m_Nodes[ spring.nNode[ 1 ] ];
|
|
if ( node1.invMass + node0.invMass < 1e-9f )
|
|
{
|
|
continue; // skip springs between pinned points
|
|
}
|
|
|
|
//float flAveDamping = ( node0.integrator.flPointDamping + node1.integrator.flPointDamping ) * 0.5f;
|
|
|
|
float expd0 = expf( -node0.integrator.flPointDamping ), expd1 = expf( -node1.integrator.flPointDamping );
|
|
// if particles are severely damped, or are massive, the spring should feel proportionally weaker:
|
|
// the original Dota spring constants had dimentionality to compute force (not acceleration), so they
|
|
// effectively (implicitly) were divided by mass to compute acceleration later
|
|
spring.flSpringConstant = m_Springs[ i ].flSpringConstant * ( node1.invMass * expd1 + node0.invMass * expd0 );
|
|
spring.flSpringDamping = m_Springs[ i ].flSpringDamping * ( node1.invMass * expd1 + node0.invMass * expd0 );
|
|
|
|
if ( spring.flSpringConstant == 0 && spring.flSpringDamping == 0 )
|
|
{
|
|
continue; // this spring will never generate any force...
|
|
}
|
|
|
|
spring.flSpringRestLength = ( m_Nodes[ spring.nNode[ 0 ] ].transform.m_vPosition - m_Nodes[ spring.nNode[ 1 ] ].transform.m_vPosition ).Length();
|
|
|
|
// make well-damped node to have effectively higher mass (lower invMass)
|
|
// so, node0 damping greater => node0 invMass lower
|
|
// because damping is an exponential function, this should figure out fine
|
|
// also, node0 fixed => weight0 == 0, and vice versa
|
|
spring.flNodeWeight0 = SnapWeight01( node0.invMass * expd0 / ( node1.invMass * expd1 + node0.invMass * expd0 ) );
|
|
created.Insert( spring.nNode[ 0 ] + ( uint( spring.nNode[ 1 ] ) << 16 ) );
|
|
created.Insert( spring.nNode[ 1 ] + ( uint( spring.nNode[ 0 ] ) << 16 ) );
|
|
|
|
springs.AddToTail( spring );
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
void CFeModelBuilder::BuildQuads( CUtlVector< FeQuad_t > &quads, bool bSkipTris )
|
|
{
|
|
quads.EnsureCapacity( m_Elems.Count( ) );
|
|
uint nQuadCount[ 3 ] = { 0, 0, 0 };
|
|
uint nLastStaticNodes = 2;
|
|
for ( int nElem = 0; nElem < m_Elems.Count( ); ++nElem )
|
|
{
|
|
const BuildElem_t &buildElem = m_Elems[ nElem ];
|
|
|
|
bool bIsQuad = buildElem.nNode[ 2 ] != buildElem.nNode[ 3 ];
|
|
if ( bSkipTris && !bIsQuad )
|
|
{
|
|
continue; // skip this quad: it's really a triangle and we skip triangles
|
|
}
|
|
|
|
uint nStaticNodes = buildElem.nStaticNodes;
|
|
Assert( nStaticNodes <= nLastStaticNodes ); // quads should already be sorted
|
|
nLastStaticNodes = nStaticNodes;
|
|
FeQuad_t quad;
|
|
quad.flSlack = buildElem.flSlack;
|
|
|
|
BuildNode_t node[ 4 ];
|
|
for ( uint j = 0; j < 4; ++j )
|
|
{
|
|
node[ j ] = m_Nodes[ buildElem.nNode[ j ] ];
|
|
}
|
|
|
|
float flSumMass = 0;
|
|
float flMass[ 4 ];
|
|
for ( uint j = nStaticNodes; j < 4; ++j )
|
|
{
|
|
flSumMass += ( flMass[ j ] = 1.0f / Max( 1e-6f, node[ j ].invMass ) );
|
|
}
|
|
|
|
Vector vCoM = vec3_origin;
|
|
float flWeight[ 4 ] = { 0,0,0,0}, flSumWeights = 0;
|
|
for ( uint j = nStaticNodes; j < 4; ++j )
|
|
{
|
|
flWeight[ j ] = flMass[ j ] / flSumMass;
|
|
vCoM += flWeight[ j ] * node[ j ].transform.m_vPosition;
|
|
flSumWeights += flWeight[ j ];
|
|
}
|
|
|
|
const VectorAligned &p0 = node[ 0 ].transform.m_vPosition;
|
|
const VectorAligned &p1 = node[ 1 ].transform.m_vPosition;
|
|
const VectorAligned &p2 = node[ 2 ].transform.m_vPosition;
|
|
const VectorAligned &p3 = node[ 3 ].transform.m_vPosition;
|
|
|
|
// special case: the center of mass is assumed to be at or between the infinite-mass nodes
|
|
switch ( nStaticNodes )
|
|
{
|
|
case 1:
|
|
vCoM = p0;
|
|
break;
|
|
case 2:
|
|
vCoM = ( p0 + p1 ) * 0.5f;
|
|
break;
|
|
}
|
|
|
|
CFeBasis basis;
|
|
if ( nStaticNodes == 2 )
|
|
{
|
|
basis.Init( p1 - p0, p2 + p3 - 2 * p0 );
|
|
}
|
|
else
|
|
{
|
|
basis.Init( p2 - p0, p3 - p1 );
|
|
}
|
|
|
|
Vector vShapeWorld[ 4 ];
|
|
for ( uint j = 0; j < 4; ++j )
|
|
{
|
|
vShapeWorld[ j ] = node[ j ].transform.m_vPosition - vCoM;
|
|
quad.nNode[ j ] = buildElem.nNode[ j ];
|
|
quad.vShape[ j ].Init( basis.WorldToLocal( vShapeWorld[ j ] ), j < nStaticNodes ? 0.0f : SnapWeight0( flWeight[ j ] / flSumWeights ) );
|
|
}
|
|
|
|
quads.AddToTail( quad );
|
|
nQuadCount[ nStaticNodes ] = quads.Count( );
|
|
}
|
|
|
|
m_nQuadCount[ 0 ] = quads.Count( );
|
|
m_nQuadCount[ 1 ] = Max( nQuadCount[ 1 ], nQuadCount[ 2 ] );
|
|
m_nQuadCount[ 2 ] = nQuadCount[ 2 ];
|
|
|
|
}
|
|
|
|
|
|
FeTri_t CFeModelBuilder::BuildTri( const BuildElem_t &buildElem, int nTriStaticNodes, int nSubTri )
|
|
{
|
|
FeTri_t tri;
|
|
BuildNode_t node[ 3 ];
|
|
for ( uint j = 0; j < 3; ++j )
|
|
{
|
|
tri.nNode[ j ] = buildElem.nNode[ j > 0 ? ( j + nSubTri ) : 0 ];
|
|
node[ j ] = m_Nodes[ tri.nNode[ j ] ];
|
|
}
|
|
|
|
float flSumMass = 0;
|
|
float flMass[ 3 ] = { 0, 0, 0 };
|
|
for ( uint j = nTriStaticNodes; j < 3; ++j )
|
|
{
|
|
flSumMass += ( flMass[ j ] = 1.0f / Max( 1e-6f, node[ j ].invMass ) );
|
|
}
|
|
|
|
Vector vEdge1 = node[ 1 ].transform.m_vPosition - node[ 0 ].transform.m_vPosition, vEdge2 = node[ 2 ].transform.m_vPosition - node[ 0 ].transform.m_vPosition;
|
|
CFeTriBasis basis( vEdge1, vEdge2 );
|
|
AssertDbg( fabsf( DotProduct( basis.vAxisY, vEdge1 ) ) < 1e-4f * ( 1 + vEdge1.Length() ) );
|
|
AssertDbg( CrossProduct( basis.vAxisX, vEdge1 ).Length( ) < 1e-4f * ( 1 + vEdge1.Length() ) );
|
|
|
|
tri.v1x = vEdge1.Length( );
|
|
tri.v2.x = DotProduct( vEdge2, basis.vAxisX );
|
|
tri.v2.y = DotProduct( vEdge2, basis.vAxisY );
|
|
tri.w1 = flMass[ 1 ] / flSumMass;
|
|
tri.w2 = flMass[ 2 ] / flSumMass;
|
|
|
|
return tri;
|
|
}
|
|
|
|
|
|
|
|
void CFeModelBuilder::BuildTris( CUtlVector< FeTri_t > &trisOut, bool bTriangulate )
|
|
{
|
|
CUtlVector< FeTri_t > tris[ 3 ];
|
|
tris[ 0 ].EnsureCapacity( m_Elems.Count( ) * 2 );
|
|
for ( int nElem = 0; nElem < m_Elems.Count( ); ++nElem )
|
|
{
|
|
const BuildElem_t &buildElem = m_Elems[ nElem ];
|
|
bool bIsQuad = buildElem.nNode[ 2 ] != buildElem.nNode[ 3 ];
|
|
if ( !bTriangulate && bIsQuad )
|
|
{
|
|
// this is a quad, skip
|
|
continue;
|
|
}
|
|
|
|
tris[ buildElem.nStaticNodes ].AddToTail( BuildTri( buildElem, buildElem.nStaticNodes, 0 ) );
|
|
|
|
if ( bIsQuad )
|
|
{
|
|
// add the second triangle of the quad ; it's 1 static node if the quad had 2 static nodes
|
|
uint nTri2static = Min< uint >( 1u, buildElem.nStaticNodes );
|
|
tris[ nTri2static ].AddToTail( BuildTri( buildElem, nTri2static, 1 ) );
|
|
}
|
|
}
|
|
|
|
m_nTriCount[ 0 ] = tris[ 0 ].Count( ) + tris[ 1 ].Count( ) + tris[ 2 ].Count( );
|
|
m_nTriCount[ 1 ] = tris[ 1 ].Count( ) + tris[ 2 ].Count( );
|
|
m_nTriCount[ 2 ] = tris[ 2 ].Count( );
|
|
|
|
trisOut.EnsureCapacity( m_nTriCount[ 0 ] );
|
|
trisOut.AddVectorToTail( tris[ 2 ] );
|
|
trisOut.AddVectorToTail( tris[ 1 ] );
|
|
trisOut.AddVectorToTail( tris[ 0 ] );
|
|
}
|
|
|
|
|
|
void CFeModelBuilder::BuildFeEdgeDesc( )
|
|
{
|
|
struct Side_t
|
|
{
|
|
uint16 nNode[ 2 ];
|
|
uint16 nElem;
|
|
};
|
|
CUtlHashtable< uint32, Side_t > edgeToVert;
|
|
uint nNodeCount = ( uint )m_Nodes.Count(); NOTE_UNUSED( nNodeCount );
|
|
for ( int nElement = 0; nElement < m_Elems.Count( ); ++nElement )
|
|
{
|
|
BuildElem_t elem = m_Elems[ nElement ];
|
|
const uint nElemEdgeCount = 4;
|
|
|
|
for ( uint nElemEdge = 0; nElemEdge < nElemEdgeCount; ++nElemEdge )
|
|
{
|
|
uint j1 = ( nElemEdge + 1 ) % nElemEdgeCount;
|
|
if ( nElemEdge == j1 )
|
|
continue; // skip degenerate edges
|
|
uint32 n0 = elem.nNode[ nElemEdge ], n1 = elem.nNode[ j1 ];
|
|
uint nEdgeV0 = n0 | ( n1 << 16 ), nEdgeV1 = n1 | ( n0 << 16 );
|
|
int j2 = ( nElemEdge + 2 ) % nElemEdgeCount, j3 = ( nElemEdge + 3 ) % nElemEdgeCount;
|
|
uint n2 = elem.nNode[ j2 ], n3 = elem.nNode[ j3 ];
|
|
Assert( n3 < nNodeCount && n2 < nNodeCount && n1 < nNodeCount && n0 < nNodeCount );
|
|
UtlHashHandle_t hFind = edgeToVert.Find( nEdgeV0 );
|
|
|
|
uint nEdgeVidx = 0;
|
|
if ( hFind == edgeToVert.InvalidHandle( ) )
|
|
{
|
|
hFind = edgeToVert.Find( nEdgeV1 );
|
|
nEdgeVidx = 1;
|
|
}
|
|
if ( hFind == edgeToVert.InvalidHandle( ) )
|
|
{
|
|
Side_t side = { { (uint16)n2, (uint16)n3 }, (uint16)nElement };
|
|
edgeToVert.Insert( nEdgeV0, side );
|
|
}
|
|
else
|
|
{
|
|
// found a matching edge!
|
|
FeEdgeDesc_t edge;
|
|
edge.nEdge[ 0 ] = n0;
|
|
edge.nEdge[ 1 ] = n1;
|
|
edge.nSide[ 0 ][ 0 ] = n2;
|
|
edge.nSide[ 0 ][ 1 ] = n3;
|
|
edge.nSide[ 1 ][ 0 ] = edgeToVert[ hFind ].nNode[ nEdgeVidx ];
|
|
edge.nSide[ 1 ][ 1 ] = edgeToVert[ hFind ].nNode[ 1 - nEdgeVidx ];
|
|
edge.nElem[ 0 ] = nElement;
|
|
edge.nElem[ 1 ] = edgeToVert[ hFind ].nElem;
|
|
|
|
m_FeEdgeDesc.AddToTail( edge );
|
|
|
|
edgeToVert.RemoveByHandle( hFind );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void CFeModelBuilder::BuildNodeSlack( float flSlackMultiplier )
|
|
{
|
|
for ( int nNode = 0; nNode < m_Nodes.Count( ); ++nNode )
|
|
{
|
|
m_Nodes[ nNode ].flSlack = FLT_MAX;
|
|
}
|
|
for ( int nElem = 0; nElem < m_Elems.Count( ); ++nElem )
|
|
{
|
|
BuildElem_t elem = m_Elems[ nElem ];
|
|
for ( int i = 0; i < 4; ++i )
|
|
{
|
|
int i1 = ( i + 1 ) % 4;
|
|
uint n0 = elem.nNode[ i ], n1 = elem.nNode[ i1 ];
|
|
if ( n0 != n1 )
|
|
{
|
|
float flSlack = flSlackMultiplier * ( m_Nodes[ n0 ].transform.m_vPosition - m_Nodes[ n1 ].transform.m_vPosition ).Length( );
|
|
m_Nodes[ n0 ].flSlack = Min( m_Nodes[ n0 ].flSlack, flSlack );
|
|
m_Nodes[ n1 ].flSlack = Min( m_Nodes[ n0 ].flSlack, flSlack );
|
|
}
|
|
}
|
|
}
|
|
for ( int nElem = 0; nElem < m_Elems.Count( ); ++nElem )
|
|
{
|
|
BuildElem_t &elem = m_Elems[ nElem ];
|
|
elem.flSlack = FLT_MAX;
|
|
for ( int i = 0; i < 4; ++i )
|
|
{
|
|
elem.flSlack = Min( elem.flSlack, m_Nodes[ elem.nNode[ i ] ].flSlack );
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void CFeModelBuilder::BuildOldFeEdges( )
|
|
{
|
|
for ( int ei = 0; ei < m_FeEdgeDesc.Count( ); ++ei )
|
|
{
|
|
// found a matching edge!
|
|
FeEdgeDesc_t edgeIn = m_FeEdgeDesc[ ei ];
|
|
OldFeEdge_t edge;
|
|
edge.m_nNode[ 0 ] = edgeIn.nEdge[ 0 ];
|
|
edge.m_nNode[ 1 ] = edgeIn.nEdge[ 1 ];
|
|
edge.m_nNode[ 2 ] = edgeIn.nSide[ 0 ][ 0 ];
|
|
edge.m_nNode[ 3 ] = edgeIn.nSide[ 1 ][ 0 ];
|
|
|
|
Vector x[ 4 ];
|
|
for ( int k = 0; k < 4; ++k )
|
|
x[ k ] = m_Nodes[ edge.m_nNode[ k ] ].transform.m_vPosition;
|
|
Vector e[ 5 ] = { x[ 1 ] - x[ 0 ], x[ 2 ] - x[ 0 ], x[ 3 ] - x[ 0 ], x[ 2 ] - x[ 1 ], x[ 3 ] - x[ 1 ] };
|
|
float cot0[ 5 ];
|
|
|
|
// this is the factor f to make Q = fK^t * fK, see "Discrete IBM: Implementation", page 2 of "A Quadratic Bending Model for Inextensible Surfaces"
|
|
float flInvAreasSqrt = sqrtf( 6.0f / ( CrossProduct( e[ 0 ], e[ 1 ] ).Length( ) + CrossProduct( e[ 0 ], e[ 2 ] ).Length( ) ) );
|
|
|
|
for ( int k = 0; k < 5; ++k )
|
|
cot0[ k ] = DotProduct( e[ 0 ], e[ k ] ) / CrossProduct( e[ 0 ], e[ k ] ).Length( ); // ( cosine / sine ), coincidentally a stable way to find a non-zero (non-degenerate) angle..
|
|
|
|
edge.m_flK[ 0 ] = flInvAreasSqrt * ( cot0[ 3 ] + cot0[ 4 ] );
|
|
edge.m_flK[ 1 ] = flInvAreasSqrt * ( cot0[ 1 ] + cot0[ 2 ] );
|
|
edge.m_flK[ 2 ] = flInvAreasSqrt * ( -cot0[ 1 ] - cot0[ 3 ] );
|
|
|
|
Vector n1 = CrossProduct( e[ 0 ], e[ 1 ] ), n2 = CrossProduct( e[ 0 ], e[ 2 ] );
|
|
float e_sqr = e[ 0 ].LengthSqr( ), n1_len = n1.Length( ), n2_len = n2.Length( );
|
|
float flTheta0 = atan2f( CrossProduct( n1, n2 ).Length( ), DotProduct( n1, n2 ) );
|
|
edge.flThetaRelaxed = sinf( 0.5f * flTheta0 );
|
|
edge.flThetaFactor = e_sqr / ( n1_len + n2_len );
|
|
edge.t = 0.5f * DotProduct( e[ 2 ] + e[ 1 ], e[ 0 ] ) / e[ 0 ].LengthSqr( );
|
|
edge.invA = flInvAreasSqrt;
|
|
edge.c01 = cot0[ 1 ];
|
|
edge.c02 = cot0[ 2 ];
|
|
edge.c03 = cot0[ 3 ];
|
|
edge.c04 = cot0[ 4 ];
|
|
|
|
// virtual edge and axial normal
|
|
m_OldFeEdges.AddToTail( edge );
|
|
}
|
|
}
|
|
|
|
|
|
// return 1/(x0^2/invM0+x1^2/invM1)
|
|
static inline float Compute1DInvInertiaTensor( float invM0, float x0, float invM1, float x1 )
|
|
{
|
|
if ( invM0 <= 1e-8f )
|
|
{
|
|
return invM1 <= 1e-8f || fabsf( x0 ) > 1e-6f ? 0.0f : invM1 / Sqr( x1 );
|
|
}
|
|
else
|
|
{
|
|
if ( invM1 <= 1e-8f )
|
|
{
|
|
return fabsf( x1 ) > 1e-6f ? 0.0f : invM0 / Sqr( x0 );
|
|
}
|
|
else
|
|
{
|
|
return 1.0f / ( Sqr( x0 ) / invM0 + Sqr( x1 ) / invM1 );
|
|
}
|
|
}
|
|
}
|
|
|
|
static inline float ComputeEdgeCenterOfMass( float a, float b )
|
|
{
|
|
return fabsf( b ) >= 1e-8f ? a / b : 0.5f;
|
|
}
|
|
|
|
static inline float CombineInvMasses( float invM0, float invM1 )
|
|
{
|
|
if ( invM0 <= 0 || invM1 <= 0 )
|
|
return 0;
|
|
return ( invM0 * invM1 ) / ( invM0 + invM1 ); // ( m0 + m1 ) ^ -1
|
|
}
|
|
|
|
void CFeModelBuilder::BuildAxialEdges( )
|
|
{
|
|
for ( int ei = 0; ei < m_FeEdgeDesc.Count( ); ++ei )
|
|
{
|
|
// found a matching edge!
|
|
FeEdgeDesc_t edgeIn = m_FeEdgeDesc[ ei ];
|
|
FeAxialEdgeBend_t edge;
|
|
edge.nNode[ 0 ] = edgeIn.nEdge[ 0 ];
|
|
edge.nNode[ 1 ] = edgeIn.nEdge[ 1 ];
|
|
edge.nNode[ 2 ] = edgeIn.nSide[ 0 ][ 0 ];
|
|
edge.nNode[ 3 ] = edgeIn.nSide[ 0 ][ 1 ];
|
|
edge.nNode[ 4 ] = edgeIn.nSide[ 1 ][ 0 ];
|
|
edge.nNode[ 5 ] = edgeIn.nSide[ 1 ][ 1 ];
|
|
|
|
Vector x[ 4 ] = {
|
|
m_Nodes[ edge.nNode[ 0 ] ].transform.m_vPosition,
|
|
m_Nodes[ edge.nNode[ 1 ] ].transform.m_vPosition,
|
|
( m_Nodes[ edge.nNode[ 2 ] ].transform.m_vPosition + m_Nodes[ edge.nNode[ 3 ] ].transform.m_vPosition )* 0.5f,
|
|
( m_Nodes[ edge.nNode[ 4 ] ].transform.m_vPosition + m_Nodes[ edge.nNode[ 5 ] ].transform.m_vPosition )* 0.5f
|
|
};
|
|
|
|
// do we need to flip it? The direction of the bend depends on the ordering of nodes
|
|
{
|
|
Vector vApproximateAxis = ( x[ 1 ] + x[ 0 ] ) - ( x[ 2 ] + x[ 3 ] );
|
|
Vector vEdge = x[ 1 ] - x[ 0 ], vVirtualEdge = x[ 2 ] - x[ 3 ];
|
|
Vector vCrossEdges = CrossProduct( vEdge, vVirtualEdge );
|
|
if ( DotProduct( vApproximateAxis, vCrossEdges ) < 0 )
|
|
{
|
|
// flip one of the edges around
|
|
Swap( x[ 0 ], x[ 1 ] );
|
|
Swap( edge.nNode[ 0 ], edge.nNode[ 1 ] );
|
|
}
|
|
}
|
|
|
|
float invM[ 4 ] = {
|
|
m_Nodes[ edge.nNode[ 0 ] ].invMass,
|
|
m_Nodes[ edge.nNode[ 1 ] ].invMass,
|
|
edge.nNode[ 2 ] == edge.nNode[ 3 ] ? m_Nodes[ edge.nNode[ 2 ] ].invMass : CombineInvMasses( m_Nodes[ edge.nNode[ 2 ] ].invMass, m_Nodes[ edge.nNode[ 3 ] ].invMass ),
|
|
edge.nNode[ 4 ] == edge.nNode[ 5 ] ? m_Nodes[ edge.nNode[ 4 ] ].invMass : CombineInvMasses( m_Nodes[ edge.nNode[ 4 ] ].invMass, m_Nodes[ edge.nNode[ 5 ] ].invMass )
|
|
};
|
|
if ( invM[ 0 ] + invM[ 1 ] + invM[ 2 ] + invM[ 3 ] == 0 )
|
|
{
|
|
// nothing to keep bent, the static nodes at the fringes will take care of it
|
|
continue;
|
|
}
|
|
|
|
Vector e[ 5 ] = { x[ 1 ] - x[ 0 ], x[ 2 ] - x[ 0 ], x[ 3 ] - x[ 0 ], x[ 2 ] - x[ 1 ], x[ 3 ] - x[ 1 ] };
|
|
float cot0[ 5 ] = { 0 };
|
|
|
|
// this is the factor f to make Q = fK^t * fK, see "Discrete IBM: Implementation", page 2 of "A Quadratic Bending Model for Inextensible Surfaces"
|
|
//float flInvAreasSqrt = sqrtf( 6.0f / ( CrossProduct( e[ 0 ], e[ 1 ] ).Length() + CrossProduct( e[ 0 ], e[ 2 ] ).Length() ) );
|
|
|
|
for ( int k = 1; k < 5; ++k )
|
|
{
|
|
cot0[ k ] = DotProduct( e[ 0 ], e[ k ] ) / CrossProduct( e[ 0 ], e[ k ] ).Length(); // ( cosine / sine ), coincidentally a stable way to find a non-zero (non-degenerate) angle..
|
|
}
|
|
|
|
Vector n1 = CrossProduct( e[ 0 ], e[ 1 ] ), n2 = CrossProduct( e[ 0 ], e[ 2 ] );
|
|
|
|
// virtual edge and axial normal
|
|
Vector eV = x[ 3 ] - x[ 2 ], an = CrossProduct( e[ 0 ], eV );
|
|
float flAxialNormalLength = an.Length( );
|
|
if ( flAxialNormalLength < 0.001f )
|
|
continue;
|
|
|
|
an /= flAxialNormalLength;
|
|
float e0len = e[ 0 ].Length( ), eVlen = eV.Length( );
|
|
Vector e0dir = e[ 0 ] / e0len;
|
|
Vector side = CrossProduct( an, e0dir );
|
|
float t2 = DotProduct( side, e[ 1 ] ), t3 = DotProduct( side, e[ 2 ] );
|
|
// the indices are a misnomer; t correspond to x, e to e in "Discrete Quadratic Curvature Energies" by Wardetzky et al, page 28
|
|
edge.tv = -t2 / ( t3 - t2 );
|
|
Vector v23 = e[ 1 ] + eV * edge.tv;
|
|
float f0len = DotProduct( v23, e0dir );
|
|
edge.te = f0len / e0len;
|
|
Vector v01 = e0dir * f0len;
|
|
|
|
Assert( fabs( DotProduct( v01 - v23, e[ 0 ] ) ) < 0.001f && fabs( DotProduct( v01 - v23, eV ) ) < 0.001f );
|
|
edge.flDist = ( v01 - v23 ).Length( );
|
|
|
|
float c0len = ComputeEdgeCenterOfMass( e0len * invM[ 0 ], invM[ 0 ] + invM[ 1 ] ); // pretend x0 = 0, x1 = e0len, find the center of mass along that line
|
|
float cVlen = ComputeEdgeCenterOfMass( eVlen * invM[ 2 ], invM[ 2 ] + invM[ 3 ] ); // pretend x2 = 0, x3 = eVlen, find the center of mass of the virtual edge
|
|
|
|
// Inv Inertia Tensor is ( m0 * x0^2 + m1 * x1^2 )^-1 == m0^-1 * m1^-1 / ( m1^-1 * x0^2 + m0^-1 * x1^2 ) , we just compute it here
|
|
float flI0inv = Compute1DInvInertiaTensor( invM[ 0 ], ( c0len ), invM[ 1 ], ( e0len - c0len ) );
|
|
float flIVinv = Compute1DInvInertiaTensor( invM[ 2 ], ( cVlen ), invM[ 3 ], ( eVlen - cVlen ) );
|
|
|
|
float /*f0len = edge.t01 * e0len,*/ fVlen = edge.tv * eVlen;
|
|
float f0 = f0len - c0len, fV = fVlen - cVlen;
|
|
|
|
if ( 1 )
|
|
{
|
|
// when applying impulse 1.0 to edge 0-1, and -1.0 to virtual edge 2-3, this is how much the x[.] vertices move
|
|
float flReact[ 4 ] = {
|
|
invM[ 0 ] - flI0inv * f0 * c0len, invM[ 1 ] + flI0inv * f0 * ( e0len - c0len ),
|
|
-( invM[ 2 ] - flIVinv * fV * cVlen ), -( invM[ 3 ] + flIVinv * fV * ( eVlen - cVlen ) )
|
|
};
|
|
|
|
// reactions at the fulcrum points
|
|
float flReact01 = edge.te * flReact[ 0 ] + ( 1 - edge.te ) * flReact[ 1 ], flReact23 = edge.tv * flReact[ 2 ] + ( 1 - edge.tv ) * flReact[ 3 ];
|
|
float flReactSumAtFulcrum = flReact01 - flReact23; // applying impulse 1.0 at the connection between fulcrums, this is how much the gap closes
|
|
if ( flReactSumAtFulcrum > 1e-8f )
|
|
{
|
|
edge.flWeight[ 0 ] = ( flReact[ 0 ] / flReactSumAtFulcrum );
|
|
edge.flWeight[ 1 ] = ( flReact[ 1 ] / flReactSumAtFulcrum );
|
|
edge.flWeight[ 2 ] = ( flReact[ 2 ] / flReactSumAtFulcrum );
|
|
//if ( edge.nNode[ 2 ] == edge.nNode[ 3 ] )
|
|
// edge.flWeight[ 2 ] *= 0.5f; // the same weight will be applied to the same node twice.. Careful! SIMD implementation doesn't need *=0.5! and scalar can easily avoid this, too
|
|
edge.flWeight[ 3 ] = ( flReact[ 3 ] / flReactSumAtFulcrum );
|
|
//if ( edge.nNode[ 4 ] == edge.nNode[ 5 ] )
|
|
// edge.flWeight[ 3 ] *= 0.5f; // the same weight will be applied to the same node twice.. Careful! SIMD implementation doesn't need *=0.5! and scalar can easily avoid this, too
|
|
|
|
|
|
Assert( invM[ 0 ] == 0 || invM[ 1 ] == 0 || invM[ 2 ] == 0 || invM[ 3 ] == 0 || fabsf( edge.flWeight[ 0 ] / invM[ 0 ] + edge.flWeight[ 1 ] / invM[ 1 ] + edge.flWeight[ 2 ] / invM[ 2 ] + edge.flWeight[ 3 ] / invM[ 3 ] ) < 1e-5f / ( invM[ 0 ] + invM[ 1 ] + invM[ 2 ] + invM[ 3 ] ) );
|
|
|
|
m_AxialEdges.AddToTail( edge );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// just apply impulse along the line between the edge and virtual edge.
|
|
float flEdgeInvMass = CombineInvMasses( invM[ 0 ], invM[ 1 ] );
|
|
// virtual edge will move against the direction, main edge will move along
|
|
float flVirtEdgeReact = invM[ 2 ] * ( 1 - edge.tv ) + invM[ 3 ] * edge.tv;
|
|
float flTotalReaction = flEdgeInvMass + flVirtEdgeReact; // this is how much the edge and virt edge close when applying impulse 1.0
|
|
if ( flTotalReaction < FLT_EPSILON )
|
|
continue; // we have nothing to move except one of the vertices on the real edge, and that'll be taken care of by quad contraints
|
|
edge.flWeight[ 0 ] = edge.flWeight[ 1 ] = flEdgeInvMass / flTotalReaction;
|
|
edge.flWeight[ 2 ] = - invM[ 2 ] / flTotalReaction;
|
|
edge.flWeight[ 3 ] = -invM[ 3 ] / flTotalReaction;
|
|
}
|
|
}
|
|
|
|
CFeModelBuilder *pThis = this;
|
|
HeapSort( m_AxialEdges, [ pThis ]( const FeAxialEdgeBend_t &left, const FeAxialEdgeBend_t &right )
|
|
{
|
|
return pThis->GetRank( left ) < pThis->GetRank( right );
|
|
}
|
|
);
|
|
}
|
|
|
|
int CFeModelBuilder::ReconcileElemStaticNodes()
|
|
{
|
|
int nExtraStaticNodesFound = 0;
|
|
// find out if any elements have 3+ static nodes and remove them; sort nodes in elements that have more static nodes than declared
|
|
// we may FastRemove elements during this loop, so counting downwards is easiest
|
|
for ( int nElem = m_Elems.Count(); nElem-- > 0; )
|
|
{
|
|
BuildElem_t &elem = m_Elems[ nElem ];
|
|
int nExtraStatics = 0;
|
|
{
|
|
uint numNodes = elem.NumNodes();
|
|
for ( uint j = elem.nStaticNodes; j < numNodes; ++j )
|
|
{
|
|
uint nNode = elem.nNode[ j ];
|
|
if ( !m_Nodes[ nNode ].bSimulated )
|
|
{
|
|
++nExtraStatics;
|
|
}
|
|
}
|
|
}
|
|
|
|
nExtraStaticNodesFound += nExtraStatics;
|
|
elem.nStaticNodes += nExtraStatics;
|
|
if ( nExtraStatics )
|
|
{
|
|
CUtlVectorAligned< BuildNode_t > &nodes = m_Nodes;
|
|
// element is still dynamic; sort it and go on
|
|
uint numNodes = elem.NumNodes();
|
|
BubbleSort( elem.nNode, numNodes, [ &nodes ]( uint nLeftNode, uint nRightNode ) {
|
|
return int( nodes[ nLeftNode ].bSimulated ) < int( nodes[ nRightNode ].bSimulated ); // sort: non-simulated, then simulated
|
|
} );
|
|
elem.nNode[ 3 ] = elem.nNode[ numNodes - 1 ];
|
|
|
|
for ( elem.nStaticNodes = 0; elem.nStaticNodes < numNodes && !nodes[ elem.nNode[ elem.nStaticNodes ] ].bSimulated ; ++elem.nStaticNodes )
|
|
continue;
|
|
}
|
|
|
|
if ( elem.nStaticNodes >= 3 )
|
|
{
|
|
// the whole element is static; remove it, mark all as static
|
|
for ( uint j = elem.nStaticNodes; j < BuildElem_t::MAX_NODES; ++j )
|
|
{
|
|
BuildNode_t &refNode = m_Nodes[ elem.nNode[ j ] ];
|
|
if ( refNode.bSimulated && !refNode.bForceSimulated )
|
|
{
|
|
refNode.bSimulated = false;
|
|
nExtraStaticNodesFound++;
|
|
elem.nStaticNodes++;
|
|
}
|
|
}
|
|
// Note: we still need fully static elements to construct stiffness rods, perhaps some face angle constraints, and the analogs of 3-static-node quads
|
|
//m_Elems.FastRemove( nElem );
|
|
}
|
|
}
|
|
return nExtraStaticNodesFound;
|
|
}
|
|
|
|
int CFeModelBuilder::RemoveFullyStaticElems()
|
|
{
|
|
int nRemoved = 0;
|
|
for ( int nElem = m_Elems.Count(); nElem-- > 0; )
|
|
{
|
|
BuildElem_t &elem = m_Elems[ nElem ];
|
|
if ( elem.nStaticNodes >= 3 )
|
|
{
|
|
++nRemoved;
|
|
m_Elems.Remove( nElem ); // keep the order
|
|
}
|
|
}
|
|
return nRemoved;
|
|
}
|
|
|
|
|
|
void CFeModelBuilder::BuildInvMassesAndSortNodes( )
|
|
{
|
|
// we don't remap anything, because we assume everything's downstream of this call
|
|
Assert( m_AxialEdges.IsEmpty( ) && m_OldFeEdges.IsEmpty( ) && m_KelagerBends.IsEmpty( ) && m_FeEdgeDesc.IsEmpty( ) );
|
|
|
|
CleanupElements();
|
|
|
|
const int nInvalidRank = INT_MAX;
|
|
CUtlVector< float > nodeMass;
|
|
if ( m_bEnableExplicitNodeMasses )
|
|
{
|
|
nodeMass.SetCount( m_Nodes.Count( ) );
|
|
for ( int i = 0; i < m_Nodes.Count( ); ++i )
|
|
{
|
|
if ( m_Nodes[ i ].invMass > FLT_EPSILON )
|
|
{
|
|
nodeMass[ i ] = 1.0f / m_Nodes[ i ].invMass;
|
|
}
|
|
else
|
|
{
|
|
nodeMass[ i ] = 0;
|
|
m_Nodes[ i ].bSimulated = false;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
RecomputeMasses( nodeMass );
|
|
BalanceGlobalMassMultipliers( nodeMass );
|
|
}
|
|
|
|
CUtlVector< CUtlSortVector< int >* >nodeConn; // nodes this node is connected to
|
|
nodeConn.SetCount( m_Nodes.Count( ) );
|
|
for ( int nNode = 0; nNode < m_Nodes.Count( ); ++nNode )
|
|
{
|
|
m_Nodes[ nNode ].nRank = nInvalidRank;
|
|
nodeConn[ nNode ] = new CUtlSortVector< int >( ); // this shouldn't normally be more than 8 elements, so even sorting this vector isn't a win, but the utility functions on Sortvector are convenient
|
|
}
|
|
|
|
// propagate static nodes
|
|
do
|
|
{
|
|
// mark all nodes declared as static in Finite ELements, as static
|
|
for ( int nElem = 0; nElem < m_Elems.Count( ); ++nElem )
|
|
{
|
|
const BuildElem_t &elem = m_Elems[ nElem ];
|
|
for ( uint j = 0; j < elem.nStaticNodes; ++j )
|
|
{
|
|
uint nNode = elem.nNode[ j ];
|
|
m_Nodes[ nNode ].bSimulated = false;
|
|
}
|
|
}
|
|
}
|
|
while ( ReconcileElemStaticNodes() );
|
|
|
|
// slam nodes that are not simulated
|
|
for ( int i = 0; i < m_Nodes.Count( ); ++i )
|
|
{
|
|
if ( !m_Nodes[ i ].bSimulated )
|
|
{
|
|
nodeMass[ i ] = 0.0f;
|
|
m_Nodes[ i ].invMass = 0.0f;
|
|
}
|
|
}
|
|
|
|
// add node connectivity from elements
|
|
for ( int nElem = 0; nElem < m_Elems.Count(); ++nElem)
|
|
{
|
|
const BuildElem_t &elem = m_Elems[ nElem ];
|
|
|
|
if ( CountSimulatedNodesIn( elem ) >= 1 )
|
|
{
|
|
AssertDbg( elem.nNode[ 1 ] != elem.nNode[ 0 ] && elem.nNode[ 2 ] != elem.nNode[ 1 ] && elem.nNode[ 0 ] != elem.nNode[ 2 ] && elem.nNode[ 0 ] != elem.nNode[ 3 ] );
|
|
|
|
for ( int j = 0; j < 3; ++j )
|
|
{
|
|
for ( int k = j + 1; k < 4; ++k )
|
|
{
|
|
nodeConn[ elem.nNode[ j ] ]->InsertIfNotFound( elem.nNode[ k ] );
|
|
nodeConn[ elem.nNode[ k ] ]->InsertIfNotFound( elem.nNode[ j ] );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// add node connectivity from rods
|
|
for ( int nRod = 0; nRod < m_Rods.Count( ); )
|
|
{
|
|
const FeRodConstraint_t &rod = m_Rods[ nRod ];
|
|
if ( CountSimulatedNodesIn( rod ) >= 1 )
|
|
{
|
|
nodeConn[ rod.nNode[ 0 ] ]->InsertIfNotFound( rod.nNode[ 1 ] );
|
|
nodeConn[ rod.nNode[ 1 ] ]->InsertIfNotFound( rod.nNode[ 0 ] );
|
|
++nRod;
|
|
}
|
|
else
|
|
{
|
|
m_Rods.FastRemove( nRod );
|
|
}
|
|
}
|
|
|
|
// invert accumulated masses
|
|
if ( !m_bEnableExplicitNodeMasses )
|
|
{
|
|
bool bShouldReconcileElems = false;
|
|
for ( int nNode = 0; nNode < m_Nodes.Count( ); ++nNode )
|
|
{
|
|
BuildNode_t &refNode = m_Nodes[ nNode ];
|
|
float flMassMultiplier = refNode.flMassMultiplier;
|
|
float flMass = nodeMass[ nNode ] * ( flMassMultiplier > FLT_EPSILON ? flMassMultiplier : 1.0f ) + refNode.flMassBias;
|
|
|
|
if ( flMass > 1e-8f )
|
|
{
|
|
refNode.invMass = 1.0f / flMass;
|
|
}
|
|
else if ( refNode.bForceSimulated && refNode.bSimulated )
|
|
{
|
|
refNode.invMass = 1.0f;
|
|
}
|
|
else
|
|
{
|
|
if ( refNode.bSimulated )
|
|
{
|
|
bShouldReconcileElems = true;
|
|
}
|
|
|
|
refNode.invMass = 0;
|
|
refNode.bSimulated = false;
|
|
}
|
|
}
|
|
if ( bShouldReconcileElems )
|
|
{
|
|
ReconcileElemStaticNodes();
|
|
}
|
|
}
|
|
|
|
// set static nodes to invM = 0 (infinite mass)
|
|
m_CtrlToNode.SetCount( m_Nodes.Count( ) );
|
|
m_NodeToCtrl.Purge( );
|
|
int nInvalidNode = -1; // m_Nodes.Count( );
|
|
m_CtrlToNode.FillWithValue( nInvalidNode );
|
|
|
|
// start ranking - building static part of Node->Ctrl map
|
|
for ( int nNode = 0; nNode < m_Nodes.Count( ); ++nNode )
|
|
{
|
|
if ( !m_Nodes[ nNode ].bSimulated )
|
|
{
|
|
m_Nodes[ nNode ].nRank = 0;
|
|
m_CtrlToNode[ nNode ] = m_NodeToCtrl.AddToTail( nNode );
|
|
}
|
|
}
|
|
|
|
int nRankBegin = 0, nRankEnd = m_NodeToCtrl.Count( ), nNextRank = 1;
|
|
do
|
|
{
|
|
for ( int nParentIdx = nRankBegin; nParentIdx < nRankEnd; ++nParentIdx )
|
|
{
|
|
uint nParentNode = m_NodeToCtrl[ nParentIdx ];
|
|
CUtlSortVector< int > *pConnNodes = nodeConn[ nParentNode ];
|
|
for ( int e = 0; e < pConnNodes->Count( ); ++e )
|
|
{
|
|
uint nChildNode = ( *pConnNodes )[ e ];
|
|
if( nChildNode != nParentNode && m_CtrlToNode[ nChildNode ] == nInvalidNode )
|
|
{
|
|
// this child node isn't mapped yet
|
|
m_CtrlToNode[ nChildNode ] = m_NodeToCtrl.AddToTail( nChildNode );
|
|
m_Nodes[ nChildNode ].nRank = nNextRank;
|
|
}
|
|
}
|
|
}
|
|
|
|
nRankBegin = nRankEnd;
|
|
nRankEnd = m_NodeToCtrl.Count( );
|
|
++nNextRank;
|
|
|
|
// find all nodes connected to this node
|
|
}
|
|
while ( nRankEnd > nRankBegin );
|
|
|
|
|
|
// now add isolated islands of elements
|
|
for ( int nElem = 0; nElem < m_Elems.Count( ); ++nElem )
|
|
{
|
|
const BuildElem_t &elem = m_Elems[ nElem ];
|
|
for ( int j = 0; j < 4; ++j )
|
|
{
|
|
uint nNode = elem.nNode[ j ];
|
|
|
|
if ( m_CtrlToNode[ nNode ] == nInvalidNode )
|
|
{
|
|
// isolated node
|
|
m_Nodes[ nNode ].nRank = nNextRank;
|
|
m_CtrlToNode[ nNode ] = m_NodeToCtrl.AddToTail( nNode );
|
|
}
|
|
}
|
|
}
|
|
|
|
for ( int nRod = 0; nRod < m_Rods.Count( ); ++nRod )
|
|
{
|
|
const FeRodConstraint_t &rod = m_Rods[ nRod ];
|
|
|
|
for ( uint j = 0; j < 2; ++j )
|
|
{
|
|
uint nNode = rod.nNode[ j ];
|
|
if ( m_CtrlToNode[ nNode ] == nInvalidNode )
|
|
{
|
|
// isolated rod
|
|
m_Nodes[ nNode ].nRank = nNextRank;
|
|
m_CtrlToNode[ nNode ] = m_NodeToCtrl.AddToTail( nNode );
|
|
}
|
|
}
|
|
}
|
|
|
|
for ( int nNode = 0; nNode < m_Nodes.Count( ); ++nNode )
|
|
{
|
|
if ( m_CtrlToNode[ nNode ] == nInvalidNode )
|
|
{
|
|
m_CtrlToNode[ nNode ] = m_NodeToCtrl.AddToTail( nNode );
|
|
}
|
|
}
|
|
|
|
|
|
HeapSort( m_NodeToCtrl, [&] ( int nLeft, int nRight ) {
|
|
const BuildNode_t &left = m_Nodes[ nLeft ];
|
|
const BuildNode_t &right = m_Nodes[ nRight ];
|
|
int nLeftDynamic = left.invMass == 0.0f ? 0 : 1;
|
|
int nRightDynamic = right.invMass == 0.0f ? 0 : 1;
|
|
|
|
if ( nLeftDynamic == nRightDynamic )
|
|
{
|
|
if ( !nLeftDynamic )
|
|
{
|
|
// the first static nodes: we need "Locked" then "Free" rotation nodes.
|
|
if ( left.bFreeRotation != right.bFreeRotation )
|
|
{
|
|
return ( left.bFreeRotation ? 1 : 0 ) < ( right.bFreeRotation ? 1 : 0 );
|
|
}
|
|
}
|
|
|
|
if ( left.nRank == right.nRank )
|
|
{
|
|
return nLeft < nRight;
|
|
}
|
|
return left.nRank < right.nRank;
|
|
}
|
|
return nLeftDynamic < nRightDynamic;
|
|
} );
|
|
|
|
for ( int nNode = 0; nNode < m_NodeToCtrl.Count( ); ++nNode )
|
|
{
|
|
m_CtrlToNode[ m_NodeToCtrl[ nNode ] ] = nNode;
|
|
}
|
|
|
|
|
|
// permutation of nodes
|
|
CUtlVectorAligned< BuildNode_t > newNodes;
|
|
newNodes.SetCount( m_NodeToCtrl.Count( ) );
|
|
for ( int i = 0; i < m_NodeToCtrl.Count( ); ++i )
|
|
{
|
|
newNodes[ i ] = m_Nodes[ m_NodeToCtrl[ i ] ];
|
|
ConvertCtrlToNode( newNodes[ i ].nParent );
|
|
ConvertCtrlToNode( newNodes[ i ].nFollowParent );
|
|
}
|
|
for ( int i = 0; i < m_CollisionSpheres.Count( ); ++i )
|
|
{
|
|
ConvertCtrlToNode( m_CollisionSpheres[ i ].m_nChild );
|
|
ConvertCtrlToNode( m_CollisionSpheres[ i ].m_nParent );
|
|
}
|
|
for ( int i = 0; i < m_CollisionPlanes.Count(); ++i )
|
|
{
|
|
BuildCollisionPlane_t &plane = m_CollisionPlanes[ i ];
|
|
ConvertCtrlToNode( plane.m_nChild );
|
|
ConvertCtrlToNode( plane.m_nParent );
|
|
}
|
|
for ( int i = 0; i < m_PresetNodeBases.Count(); ++i )
|
|
{
|
|
FeNodeBase_t &nb = m_PresetNodeBases[ i ];
|
|
ConvertCtrlToNode( nb.nNode );
|
|
ConvertCtrlToNode( nb.nNodeX0 );
|
|
ConvertCtrlToNode( nb.nNodeX1 );
|
|
ConvertCtrlToNode( nb.nNodeY0 );
|
|
ConvertCtrlToNode( nb.nNodeY1 );
|
|
}
|
|
for ( int i = 0; i < m_TaperedCapsuleStretches.Count(); ++i )
|
|
{
|
|
FeTaperedCapsuleStretch_t &tc = m_TaperedCapsuleStretches[ i ];
|
|
ConvertCtrlToNode( tc.nNode[ 0 ] );
|
|
ConvertCtrlToNode( tc.nNode[ 1 ] );
|
|
KeepIn01( tc.flStickiness );
|
|
if ( tc.flRadius[ 0 ] > tc.flRadius[ 1 ] )
|
|
{
|
|
Swap( tc.flRadius[ 0 ], tc.flRadius[ 1 ] );
|
|
Swap( tc.nNode[ 0 ], tc.nNode[ 1 ] );
|
|
}
|
|
}
|
|
for ( int i = m_SphereRigids.Count(); i-- > 0; )
|
|
{
|
|
FeSphereRigid_t &sr = m_SphereRigids[ i ];
|
|
ConvertCtrlToNode( sr.nNode );
|
|
KeepIn01( sr.flStickiness );
|
|
if ( sr.flRadius < 0.05f )
|
|
{
|
|
// this ball is too small to collide with anything meaningfully
|
|
m_SphereRigids.FastRemove( i );
|
|
}
|
|
}
|
|
for ( int i = m_TaperedCapsuleRigids.Count(); i-- > 0; )
|
|
{
|
|
FeTaperedCapsuleRigid_t &tc = m_TaperedCapsuleRigids[ i ];
|
|
ConvertCtrlToNode( tc.nNode );
|
|
KeepIn01( tc.flStickiness );
|
|
if ( tc.flRadius[ 0 ] > tc.flRadius[ 1 ] )
|
|
{
|
|
Swap( tc.flRadius[ 0 ], tc.flRadius[ 1 ] );
|
|
Swap( tc.vCenter[ 0 ], tc.vCenter[ 1 ] );
|
|
}
|
|
|
|
// is this a sphere?
|
|
if ( tc.flRadius[ 1 ] < 0.05f )
|
|
{
|
|
// this rod is too thin to collide meaningfully
|
|
m_TaperedCapsuleRigids.FastRemove( i );
|
|
}
|
|
else if ( ( tc.vCenter[ 0 ] - tc.vCenter[ 1 ] ).Length() <= tc.flRadius[ 1 ] - tc.flRadius[ 0 ] + 0.05f )
|
|
{
|
|
// this cone's tip hides in the base sphere; convert to a sphere
|
|
FeSphereRigid_t sr;
|
|
sr.flRadius = tc.flRadius[ 1 ];
|
|
sr.nNode = tc.nNode;
|
|
sr.nCollisionMask = tc.nCollisionMask;
|
|
m_SphereRigids.AddToTail( sr );
|
|
m_TaperedCapsuleRigids.FastRemove( i );
|
|
}
|
|
}
|
|
for ( int i = 0; i < m_FitInfluences.Count(); ++i )
|
|
{
|
|
FeFitInfluence_t &influence = m_FitInfluences[ i ];
|
|
ConvertCtrlToNode( influence.nMatrixNode );
|
|
ConvertCtrlToNode( influence.nVertexNode );
|
|
}
|
|
Assert( m_ReverseOffsets.Count() == 0 ); // Code path not tested: do we need to convert nBoneCtrl (ctrl->nodes)?
|
|
for ( int i = 0; i < m_ReverseOffsets.Count(); ++i )
|
|
{
|
|
FeNodeReverseOffset_t &revOffset = m_ReverseOffsets[ i ];
|
|
//ConvertCtrlToNode( revOffset.nBoneCtrl );
|
|
ConvertCtrlToNode( revOffset.nTargetNode );
|
|
}
|
|
m_Nodes.Swap( newNodes );
|
|
|
|
for ( m_nStaticNodes = 0, m_nRotLockStaticNodes = 0; m_nStaticNodes < m_Nodes.Count( ); m_nStaticNodes++ )
|
|
{
|
|
if ( m_Nodes[ m_nStaticNodes ].invMass > 0 )
|
|
break;
|
|
|
|
if ( !m_Nodes[ m_nStaticNodes ].bFreeRotation )
|
|
{
|
|
AssertDbg( m_nRotLockStaticNodes == m_nStaticNodes ); // if this assert fires, it means there are holes in the subarray of static non-rotating nodes
|
|
m_nRotLockStaticNodes = m_nStaticNodes + 1; // this node is locked-rotation
|
|
}
|
|
}
|
|
|
|
for ( uint i = 0; i < m_nRotLockStaticNodes; ++i )
|
|
{
|
|
AssertDbg( !m_Nodes[ i ].bFreeRotation );
|
|
}
|
|
|
|
// now that we have permuted the nodes, reindex all elements
|
|
for ( int i = 0; i < m_Elems.Count( ); ++i )
|
|
{
|
|
BuildElem_t &elem = m_Elems[ i ];
|
|
elem.nRank = INT_MAX;
|
|
for ( int j = 0; j < 4; ++j )
|
|
{
|
|
uint &refNodeIndex = elem.nNode[j];
|
|
refNodeIndex = m_CtrlToNode[ refNodeIndex ];
|
|
Assert( refNodeIndex < uint( m_Nodes.Count( ) ) );
|
|
Assert( uint( j ) < ( elem.nStaticNodes == 3 && elem.nNode[2] == elem.nNode[3] ? 4 : elem.nStaticNodes ) ? ( refNodeIndex < m_nStaticNodes ) : ( refNodeIndex >= m_nStaticNodes ) );
|
|
int nNodeRank = m_Nodes[ refNodeIndex ].nRank;
|
|
if ( nNodeRank < elem.nRank )
|
|
elem.nRank = nNodeRank;
|
|
}
|
|
}
|
|
for ( int i = 0; i < m_Rods.Count( ); ++i )
|
|
{
|
|
for ( int j = 0; j < 2; ++j )
|
|
{
|
|
m_Rods[ i ].nNode[ j ] = m_CtrlToNode[ m_Rods[ i ].nNode[ j ] ];
|
|
}
|
|
}
|
|
for ( int i = 0; i < m_Springs.Count( ); ++i )
|
|
{
|
|
for ( int j = 0; j < 2; ++j )
|
|
{
|
|
m_Springs[ i ].nNode[ j ] = m_CtrlToNode[ m_Springs[ i ].nNode[ j ] ];
|
|
}
|
|
}
|
|
|
|
|
|
|
|
std::stable_sort( m_Elems.begin(), m_Elems.end(), BuildElem_t::Order );
|
|
//HeapSort( m_Elems, BuildElem_t::Order );
|
|
|
|
// note: later we sort rods anyway
|
|
/*
|
|
HeapSort( m_Rods, [&] ( const FeRodConstraint_t &left, const FeRodConstraint_t &right ){
|
|
int nLeftRank = Min( m_Nodes[ left.nNode[ 0 ] ].nRank, m_Nodes[ left.nNode[ 1 ] ].nRank );
|
|
int nRightRank = Min( m_Nodes[ right.nNode[ 0 ] ].nRank, m_Nodes[ right.nNode[ 1 ] ].nRank );
|
|
return nLeftRank < nRightRank;
|
|
} );
|
|
*/
|
|
}
|
|
|
|
|
|
|
|
|
|
int CFeModelBuilder::CountSimulatedNodesIn( const FeRodConstraint_t & rod )
|
|
{
|
|
int nResult = 0;
|
|
for ( int i = 0; i < 2; i++ )
|
|
{
|
|
if ( m_Nodes[ rod.nNode[ i ] ].bSimulated )
|
|
{
|
|
++nResult;
|
|
}
|
|
}
|
|
return nResult;
|
|
}
|
|
|
|
|
|
|
|
int CFeModelBuilder::CountSimulatedNodesIn( const BuildElem_t& elem )
|
|
{
|
|
int nResult = 0;
|
|
for ( int i = 0; i < 4; i++ )
|
|
{
|
|
if ( i > 0 && elem.nNode[ i ] == elem.nNode[ i - 1 ] )
|
|
continue;
|
|
if ( m_Nodes[ elem.nNode[ i ] ].bSimulated )
|
|
{
|
|
++nResult;
|
|
}
|
|
}
|
|
return nResult;
|
|
}
|
|
|
|
|
|
|
|
void CFeModelBuilder::BuildCtrlOffsets( )
|
|
{
|
|
int nBenignlyParentlessVirtualNodes = 0;
|
|
for ( int nNode = 0; nNode < m_Nodes.Count( ); ++nNode )
|
|
{
|
|
const BuildNode_t &node = m_Nodes[ nNode ];
|
|
if ( !node.bVirtual )
|
|
{
|
|
continue;
|
|
}
|
|
if ( node.bSimulated && node.integrator.flAnimationForceAttraction == 0 && node.integrator.flAnimationVertexAttraction == 0 )
|
|
{
|
|
if ( !( m_nDynamicNodeFlags & FE_FLAG_ENABLE_FTL ) )
|
|
{
|
|
// this is a new-style cloth (not source1 compatible), no need to be ultra-conservative, so let's just skip the offsets for nodes that don't need it?
|
|
// unless we want to be able to dynamically tweak animation attraction in softbody
|
|
++nBenignlyParentlessVirtualNodes;
|
|
continue;
|
|
}
|
|
}
|
|
int nParentNode = node.nParent;
|
|
if ( nParentNode < 0 )
|
|
continue; // there's no parent for this virtual node, that's fine, we'll just have no animation attraction because we'll have no animation on it
|
|
|
|
while ( nParentNode >= 0 && m_Nodes[ nParentNode ].bVirtual )
|
|
{
|
|
nParentNode = m_Nodes[ nParentNode ].nParent;
|
|
if ( nParentNode == node.nParent )
|
|
{
|
|
// parent node loop detected!
|
|
nParentNode = -1;
|
|
break;
|
|
}
|
|
}
|
|
if ( nParentNode < 0 )
|
|
{
|
|
Warning( "Cannot find real parent of virtual node %d:%s\n", nNode, node.pName );
|
|
continue;
|
|
}
|
|
|
|
if ( node.bOsOffset )
|
|
{
|
|
FeCtrlOsOffset_t offset;
|
|
offset.nCtrlChild = m_NodeToCtrl[ nNode ];
|
|
offset.nCtrlParent = m_NodeToCtrl[ nParentNode ];
|
|
m_CtrlOsOffsets.AddToTail( offset );
|
|
}
|
|
else
|
|
{
|
|
FeCtrlOffset_t offset;
|
|
offset.nCtrlChild = m_NodeToCtrl[ nNode ];
|
|
offset.nCtrlParent = m_NodeToCtrl[ nParentNode ];
|
|
offset.vOffset = m_Nodes[ nParentNode ].transform.TransformVectorByInverse( node.transform.m_vPosition );
|
|
m_CtrlOffsets.AddToTail( offset );
|
|
}
|
|
}
|
|
if ( nBenignlyParentlessVirtualNodes )
|
|
{
|
|
Msg( "%d/%d cloth nodes(verts) will simulate without any animation influence.\n", nBenignlyParentlessVirtualNodes, m_Nodes.Count() );
|
|
}
|
|
// sort just to have more a cache-friendly loop and more eye-friendly list; it's not necessary per se
|
|
HeapSort( m_CtrlOffsets, [] ( const FeCtrlOffset_t &left, const FeCtrlOffset_t &right ) {
|
|
return left.nCtrlChild < right.nCtrlChild;
|
|
} );
|
|
HeapSort( m_CtrlOsOffsets, [] ( const FeCtrlOsOffset_t &left, const FeCtrlOsOffset_t &right ) {
|
|
return left.nCtrlChild < right.nCtrlChild;
|
|
} );
|
|
}
|
|
|
|
|
|
// returns the number of inclusive collision ellipsoids
|
|
uint CFeModelBuilder::BuildCollisionSpheres( CUtlVector< FeCollisionSphere_t > &collisionSpheres )
|
|
{
|
|
for ( int i = m_CollisionSpheres.Count(); i-- > 0; )
|
|
{
|
|
BuildCollisionSphere_t &sphere = m_CollisionSpheres[ i ];
|
|
if ( sphere.IsDegenerate( ) || !m_Nodes[ sphere.m_nChild ].bSimulated )
|
|
{
|
|
m_CollisionSpheres.FastRemove( i );
|
|
}
|
|
else
|
|
{
|
|
KeepIn01( sphere.m_flStickiness );
|
|
}
|
|
}
|
|
|
|
|
|
HeapSort( m_CollisionSpheres, [] ( const BuildCollisionSphere_t &left, const BuildCollisionSphere_t&right ) {
|
|
if ( left.m_bInclusive != right.m_bInclusive )
|
|
return left.m_bInclusive > right.m_bInclusive;
|
|
if ( left.m_nChild != right.m_nChild )
|
|
return left.m_nChild < right.m_nChild;
|
|
return left.m_nParent < right.m_nParent;
|
|
} );
|
|
uint nInclusive = 0;
|
|
// while ( nInclusive < uint( m_CollisionEllipsoids.Count( ) ) && m_CollisionEllipsoids[ nInclusive ].m_bInclusive )
|
|
// {
|
|
// nInclusive++;
|
|
// }
|
|
collisionSpheres.SetCount( m_CollisionSpheres.Count( ) );
|
|
for ( int i = 0; i < m_CollisionSpheres.Count( ); ++i )
|
|
{
|
|
const BuildCollisionSphere_t &ce = m_CollisionSpheres[ i ];
|
|
FeCollisionSphere_t &fce = collisionSpheres[ i ];
|
|
if ( ce.m_bInclusive )
|
|
{
|
|
nInclusive = i + 1;
|
|
fce.m_flRFactor = 1.0f / Sqr( ce.m_flRadius );
|
|
}
|
|
else
|
|
{
|
|
fce.m_flRFactor = ce.m_flRadius;
|
|
}
|
|
|
|
fce.nChildNode = ce.m_nChild;
|
|
fce.nCtrlParent = m_NodeToCtrl[ ce.m_nParent ];
|
|
fce.m_vOrigin = ce.m_vOrigin;
|
|
}
|
|
|
|
return nInclusive;
|
|
}
|
|
|
|
|
|
|
|
// returns the number of inclusive collision ellipsoids
|
|
void CFeModelBuilder::BuildCollisionPlanes( CUtlVector< FeCollisionPlane_t > &collisionPlanes )
|
|
{
|
|
for ( int i = m_CollisionPlanes.Count(); i-- > 0; )
|
|
{
|
|
BuildCollisionPlane_t &plane = m_CollisionPlanes[ i ];
|
|
if ( plane.IsDegenerate( ) || !m_Nodes[ plane.m_nChild ].bSimulated )
|
|
{
|
|
m_CollisionPlanes.FastRemove( i );
|
|
}
|
|
else
|
|
{
|
|
KeepIn01( plane.m_flStickiness );
|
|
}
|
|
}
|
|
|
|
|
|
HeapSort( m_CollisionPlanes, [] ( const BuildCollisionPlane_t &left, const BuildCollisionPlane_t&right ) {
|
|
if ( left.m_nChild != right.m_nChild )
|
|
return left.m_nChild < right.m_nChild;
|
|
return left.m_nParent < right.m_nParent;
|
|
} );
|
|
// while ( nInclusive < uint( m_CollisionEllipsoids.Count( ) ) && m_CollisionEllipsoids[ nInclusive ].m_bInclusive )
|
|
// {
|
|
// nInclusive++;
|
|
// }
|
|
collisionPlanes.SetCount( m_CollisionPlanes.Count( ) );
|
|
for ( int i = 0; i < m_CollisionPlanes.Count( ); ++i )
|
|
{
|
|
const BuildCollisionPlane_t &source = m_CollisionPlanes[ i ];
|
|
FeCollisionPlane_t &fce = collisionPlanes[ i ];
|
|
fce.nChildNode = source.m_nChild;
|
|
fce.nCtrlParent = m_NodeToCtrl[ source.m_nParent ];
|
|
fce.m_Plane = source.m_Plane;
|
|
fce.m_Plane.m_vNormal.NormalizeInPlace( );
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
void CFeModelBuilder::BuildWorldCollisionNodes( CUtlVector< FeWorldCollisionParams_t > &worldCollisionParams, CUtlVector< uint16 > &worldCollisionNodes )
|
|
{
|
|
CUtlVectorAligned< BuildNode_t > &nodes = m_Nodes;
|
|
int nAdded = 0;
|
|
worldCollisionNodes.EnsureCapacity( nodes.Count( ) );
|
|
for ( int i = 0; i < m_Nodes.Count( ); ++i )
|
|
{
|
|
if ( nodes[ i ].bWorldCollision && nodes[ i ].bSimulated )
|
|
{
|
|
nodes[ i ].flWorldFriction = Min( nodes[ i ].flWorldFriction, 1.0f );
|
|
nAdded++;
|
|
worldCollisionNodes.AddToTail( i );
|
|
}
|
|
}
|
|
|
|
if ( !nAdded )
|
|
return;
|
|
|
|
// sort by ascending "WorldFriction"; this lends itself to LOD-ing, as we can clamp out "world friction" close to "1" ("world friction" is a parameter from Source1; it's not a physical friction! )
|
|
HeapSort( worldCollisionNodes, [&nodes]( uint16 left, uint16 right ) {
|
|
if ( nodes[ left ].flWorldFriction != nodes[ right ].flWorldFriction )
|
|
{
|
|
return nodes[ left ].flWorldFriction < nodes[ right ].flWorldFriction;
|
|
}
|
|
return nodes[ left ].flGroundFriction > nodes[ right ].flGroundFriction;
|
|
} );
|
|
|
|
uint nBegin = 0;
|
|
do
|
|
{
|
|
// start the new block of parameters
|
|
FeWorldCollisionParams_t ¶ms = worldCollisionParams[ worldCollisionParams.AddToTail( ) ];
|
|
BuildNode_t &beginNode = m_Nodes[ worldCollisionNodes[ nBegin ] ];
|
|
params.flWorldFriction = beginNode.flWorldFriction;
|
|
params.flGroundFriction = beginNode.flGroundFriction;
|
|
//uint nFirstNode = worldCollisionNodes[ nBegin ];
|
|
for ( params.nListBegin = nBegin, params.nListEnd = nBegin + 1; int( params.nListEnd ) < worldCollisionNodes.Count(); ++params.nListEnd )
|
|
{
|
|
uint nNextNode = worldCollisionNodes[ params.nListEnd ];
|
|
if ( m_Nodes[ nNextNode ].flWorldFriction - params.flWorldFriction > 0.1f
|
|
|| m_Nodes[ nNextNode ].flGroundFriction - params.flGroundFriction >0.1f )
|
|
{
|
|
break; // time to start a new block of parameters
|
|
}
|
|
}
|
|
nBegin = params.nListEnd; // start the next batch of parameters
|
|
}
|
|
while ( int( nBegin ) < worldCollisionNodes.Count( ) );
|
|
}
|
|
|
|
FeFitWeight_t GetBestWeight( const CFeModelBuilder::FitWeightArray_t &array )
|
|
{
|
|
FeFitWeight_t fw = array[ 0 ];
|
|
for ( int i = 1; i < array.Count(); ++i )
|
|
{
|
|
if ( array[ i ].flWeight > fw.flWeight )
|
|
fw = array[ i ];
|
|
}
|
|
return fw;
|
|
}
|
|
|
|
|
|
struct CFitMatrixBuildInfo
|
|
{
|
|
public:
|
|
int m_nNodeIndex;
|
|
CFeModelBuilder::FitWeightArray_t m_StaticNodes;
|
|
CFeModelBuilder::FitWeightArray_t m_DynamicNodes;
|
|
CFitMatrixBuildInfo() : m_nNodeIndex( -1 ) { }
|
|
void AddNode( const FeFitWeight_t &w, bool bSimulated )
|
|
{
|
|
if ( bSimulated )
|
|
m_DynamicNodes.AddToTail( w );
|
|
else
|
|
m_StaticNodes.AddToTail( w );
|
|
}
|
|
static void Normalize( CUtlVector< FeFitWeight_t > &nodes )
|
|
{
|
|
if ( nodes.Count() )
|
|
{
|
|
float flSumWeights = 0;
|
|
for ( int nNode = 0; nNode < nodes.Count(); ++nNode )
|
|
{
|
|
FeFitWeight_t &w = nodes[ nNode ];
|
|
w.nDummy = 0;
|
|
flSumWeights += w.flWeight;
|
|
}
|
|
if ( flSumWeights <= FLT_EPSILON )
|
|
{
|
|
for ( int nNode = 0; nNode < nodes.Count(); ++nNode )
|
|
{
|
|
FeFitWeight_t &w = nodes[ nNode ];
|
|
w.flWeight = 1.0f / float( nodes.Count() );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for ( int nNode = 0; nNode < nodes.Count(); ++nNode )
|
|
{
|
|
FeFitWeight_t &w = nodes[ nNode ];
|
|
w.flWeight /= flSumWeights;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
int Finish()
|
|
{
|
|
if ( m_nNodeIndex < 0 )
|
|
{
|
|
return 0; // we need a node to influence
|
|
}
|
|
// make sure our dummies are all zeroed out and weights are all summing up to 1.0 - not necessary if we don't use stretch min/max and (bracketing of singular values of Apq*Aqq^-1 matrix)
|
|
// Warning: These normalized weights don't work with FeedbackFitTransforms()
|
|
//Normalize( m_StaticNodes );
|
|
//Normalize( m_DynamicNodes );
|
|
|
|
// if we have no influences, we have no weight node
|
|
return m_StaticNodes.Count() + m_DynamicNodes.Count( );
|
|
}
|
|
|
|
uint GetMostInfluentialNode()const
|
|
{
|
|
if ( !m_StaticNodes.IsEmpty() )
|
|
return GetBestWeight( m_StaticNodes ).nNode;
|
|
if ( !m_DynamicNodes.IsEmpty() )
|
|
return GetBestWeight( m_DynamicNodes ).nNode;
|
|
return m_nNodeIndex;
|
|
}
|
|
};
|
|
|
|
|
|
|
|
Vector CFeModelBuilder::ComputeCenter( const FitWeightArray_t &weights )
|
|
{
|
|
Vector vVertSum = vec3_origin;
|
|
float flWeightSum = 0;
|
|
for ( int i = 0; i < weights.Count(); ++i )
|
|
{
|
|
const FeFitWeight_t &w = weights[ i ];
|
|
vVertSum += w.flWeight * m_Nodes[ w.nNode ].transform.m_vPosition;
|
|
flWeightSum += w.flWeight;
|
|
}
|
|
if ( flWeightSum > FLT_EPSILON )
|
|
return vVertSum / flWeightSum;
|
|
if ( weights.Count() > 0 )
|
|
return m_Nodes[ weights[ 0 ].nNode ].transform.m_vPosition;
|
|
return vec3_origin;
|
|
}
|
|
|
|
void CFeModelBuilder::BuildFitMatrices( MbaContext_t &context )
|
|
{
|
|
CUtlVector< CFitMatrixBuildInfo* > nodeFitMatrices;
|
|
nodeFitMatrices.SetCount( m_Nodes.Count() );
|
|
nodeFitMatrices.FillWithValue( NULL );
|
|
for ( int i = 0; i < m_FitInfluences.Count(); ++i )
|
|
{
|
|
FeFitInfluence_t &inf = m_FitInfluences[ i ];
|
|
if ( inf.flWeight <= 0 )
|
|
continue;
|
|
|
|
CFitMatrixBuildInfo*&info = nodeFitMatrices[ inf.nMatrixNode ];
|
|
if ( !info )
|
|
info = new CFitMatrixBuildInfo;
|
|
info->m_nNodeIndex = inf.nMatrixNode;
|
|
FeFitWeight_t fw;
|
|
fw.flWeight = inf.flWeight;
|
|
fw.nNode = inf.nVertexNode;
|
|
fw.nDummy = 0;
|
|
info->AddNode( fw, m_Nodes[ inf.nVertexNode ].bSimulated );
|
|
}
|
|
|
|
int nPrecomputeWeightCount = 0;
|
|
for ( int i = nodeFitMatrices.Count(); i-- > 0; )
|
|
{
|
|
if ( !nodeFitMatrices[ i ] )
|
|
nodeFitMatrices.FastRemove( i );
|
|
else if ( int nWeights = nodeFitMatrices[ i ]->Finish() )
|
|
{
|
|
// we keep this one
|
|
nPrecomputeWeightCount += nWeights;
|
|
}
|
|
else
|
|
{
|
|
delete nodeFitMatrices[ i ];
|
|
nodeFitMatrices.FastRemove( i );
|
|
}
|
|
}
|
|
|
|
// sort by the count of statics
|
|
HeapSort( nodeFitMatrices, []( CFitMatrixBuildInfo*pLeft, CFitMatrixBuildInfo*pRight ){ return pLeft->m_StaticNodes.Count() > pRight->m_StaticNodes.Count(); } );
|
|
context.fitMatrices.RemoveAll();
|
|
context.fitMatrices.EnsureCapacity( nodeFitMatrices.Count() );
|
|
context.fitWeights.RemoveAll();
|
|
context.fitWeights.EnsureCapacity( nPrecomputeWeightCount );
|
|
|
|
int nTooFewInfluences = 0;
|
|
CUtlString simpleFitList, complexFitList;
|
|
int nMatricesByStaticCount[ 3 ] = { 0, 0, 0 };
|
|
for ( int i = 0; i < nodeFitMatrices.Count(); ++i )
|
|
{
|
|
CFitMatrixBuildInfo *pInfo = nodeFitMatrices[ i ];
|
|
|
|
FeFitMatrix_t fm;
|
|
fm.nNode = pInfo->m_nNodeIndex;
|
|
fm.nCtrl = m_NodeToCtrl[ fm.nNode ];
|
|
Vector vFitCenter = ComputeCenter( pInfo->m_StaticNodes.Count() ? pInfo->m_StaticNodes : pInfo->m_DynamicNodes );
|
|
fm.vCenter = vFitCenter;
|
|
/*CovMatrix3 Aqq;
|
|
Aqq.Reset();
|
|
for ( FeFitWeight_t &w : pInfo->m_DynamicNodes )
|
|
{
|
|
Aqq.AddCov( m_Nodes[ w.nNode ].transform.m_vPosition - fm.vCenter, w.flWeight );
|
|
}
|
|
fm.AqqInv = Aqq.GetPseudoInverse();*/
|
|
fm.bone.m_vPosition = m_Nodes[ fm.nNode ].transform.m_vPosition - vFitCenter;
|
|
fm.bone.m_orientation = m_Nodes[ fm.nNode ].transform.m_orientation;
|
|
//fm.flStretchMax = fm.flStretchMin = 1.0f;
|
|
|
|
if ( pInfo->m_StaticNodes.Count() + pInfo->m_DynamicNodes.Count() < m_nFitMatrixMinInfluences )
|
|
{
|
|
// need to add a few more verts
|
|
nTooFewInfluences++;
|
|
uint nBestInfluence = pInfo->GetMostInfluentialNode();
|
|
if ( nBestInfluence < uint( m_Nodes.Count() ) )
|
|
{
|
|
// add the verts from this element to control this
|
|
|
|
// compute the orientation of this node,
|
|
// except we'll just stomp that orientation over our target node:
|
|
// the node that doesn't have enough influences to shape-fit it reliably
|
|
FeNodeBase_t nodeBase = BuildNodeBasisFast( m_Nodes, fm.nNode, *( m_NodeNeighbors[ nBestInfluence ] ));
|
|
m_NodeBases.AddToTail( nodeBase );
|
|
|
|
FeNodeReverseOffset_t revOffset;
|
|
revOffset.nBoneCtrl = fm.nCtrl;
|
|
revOffset.nTargetNode = nBestInfluence;
|
|
revOffset.vOffset = m_Nodes[ fm.nNode ].transform.TransformVectorByInverse( m_Nodes[ nBestInfluence ].transform.m_vPosition );
|
|
m_ReverseOffsets.AddToTail( revOffset );
|
|
simpleFitList.Append( " " ); simpleFitList.Append( m_Nodes[ fm.nNode ].pName );
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
Warning( "Too few simulated vertices bound to matrix %s\n", m_Nodes[ fm.nNode ].pName );
|
|
}
|
|
}
|
|
context.fitWeights.AddVectorToTail( pInfo->m_StaticNodes );
|
|
fm.nBeginDynamic = context.fitWeights.Count();
|
|
context.fitWeights.AddVectorToTail( pInfo->m_DynamicNodes );
|
|
fm.nEnd = context.fitWeights.Count();
|
|
context.fitMatrices.AddToTail( fm );
|
|
nMatricesByStaticCount[ Min( pInfo->m_StaticNodes.Count(), 2 ) ] = context.fitMatrices.Count();
|
|
complexFitList.Append( " " ); complexFitList.Append( m_Nodes[ fm.nNode ].pName );
|
|
}
|
|
if ( nTooFewInfluences )
|
|
{
|
|
Msg( "%d of %d driving bones have too few influences and were switched to simplified single-polygon shape-fit mode: {%s}\n%d matrices will shape-fit complex deformations: {%s}\n",
|
|
nTooFewInfluences,
|
|
nodeFitMatrices.Count(),
|
|
simpleFitList.Get(),
|
|
context.fitMatrices.Count(),
|
|
complexFitList.Get()
|
|
);
|
|
}
|
|
|
|
// make sure [0] >= [1] >= [2]
|
|
for ( int i = 3; i-- > 1; )
|
|
nMatricesByStaticCount[ i - 1 ] = Max( nMatricesByStaticCount[ i - 1 ], nMatricesByStaticCount[ i ] );
|
|
context.m_nFitMatrices1 = nMatricesByStaticCount[ 1 ];
|
|
context.m_nFitMatrices2 = nMatricesByStaticCount[ 2 ];
|
|
|
|
nodeFitMatrices.PurgeAndDeleteElements();
|
|
}
|
|
|
|
|
|
void CFeModelBuilder::RemoveStandaloneNodeBases( MbaContext_t &context )
|
|
{
|
|
CVarBitVec needBase( m_Nodes.Count() );
|
|
for ( int i = 0; i < m_ReverseOffsets.Count(); ++i )
|
|
{
|
|
needBase.Set( CtrlToNode( m_ReverseOffsets[ i ].nBoneCtrl ) );
|
|
}
|
|
|
|
// only leave those bases that are necessary to compute before revOffsets
|
|
int nRunningCount = 0;
|
|
for ( int i = 0; i < m_NodeBases.Count(); ++i )
|
|
{
|
|
if ( needBase.IsBitSet( i ) )
|
|
{
|
|
if ( i != nRunningCount )
|
|
{
|
|
m_NodeBases[ nRunningCount ] = m_NodeBases[ i ];
|
|
}
|
|
nRunningCount++;
|
|
}
|
|
}
|
|
m_NodeBases.SetCountNonDestructively( nRunningCount );
|
|
}
|
|
|
|
|
|
void CFeModelBuilder::BuildNodeFollowers( CUtlVector< FeFollowNode_t > &nodeFollowers )
|
|
{
|
|
nodeFollowers.Purge( );
|
|
nodeFollowers.EnsureCapacity( m_Nodes.Count( ) );
|
|
for ( int i = 0; i < m_Nodes.Count( ); ++i )
|
|
{
|
|
FeFollowNode_t follower;
|
|
follower.flWeight = m_Nodes[ i ].flFollowWeight;
|
|
if ( follower.flWeight > 0.001f && m_Nodes[ i ].nFollowParent >= 0 && m_Nodes[ i ].bSimulated )
|
|
{
|
|
follower.nChildNode = i;
|
|
follower.nParentNode = m_Nodes[ i ].nFollowParent;
|
|
nodeFollowers.AddToTail( follower );
|
|
}
|
|
}
|
|
// note: we actually need to sort the followers, because following is generally order-dependent
|
|
// it shouldn't make a difference for Dota imported cloth, as there cohorts of nodes follow a single parent
|
|
// Just scan the node followers backwards here, to find if there are parents after children
|
|
CVarBitVec followers( m_Nodes.Count() );
|
|
for ( int i = nodeFollowers.Count(); i-- > 0; )
|
|
{
|
|
const FeFollowNode_t &fn = nodeFollowers[ i ];
|
|
if ( followers.IsBitSet( fn.nParentNode ) )
|
|
{
|
|
Warning( "FeModelBuilder: Follower #%d %s is following parent #%d %s, which is also a follower but is setup down the chain. The order should be parent-to-child. The follower chain may produce overly-soft or 1-tick-lagging results\n",
|
|
fn.nChildNode, m_Nodes[ fn.nChildNode ].pName, fn.nParentNode, m_Nodes[ fn.nParentNode ].pName );
|
|
}
|
|
followers.Set( fn.nChildNode );
|
|
}
|
|
}
|
|
|
|
void CFeModelBuilder::CleanupElements()
|
|
{
|
|
int nRemoved = 0;
|
|
for ( int nElem = m_Elems.Count(); nElem-- > 0; )
|
|
{
|
|
BuildElem_t &elem = m_Elems[ nElem ];
|
|
|
|
uint n0 = elem.nNode[ 0 ], n1 = elem.nNode[ 1 ], n2 = elem.nNode[ 2 ], n3 = elem.nNode[ 3 ];
|
|
Vector p0 = m_Nodes[ n0 ].transform.m_vPosition, p1 = m_Nodes[ n1 ].transform.m_vPosition, p2 = m_Nodes[ n2 ].transform.m_vPosition;
|
|
Vector vCross = CrossProduct( p1 - p0, p2 - p0 );
|
|
if ( vCross.Length() <= 1e-6f )
|
|
{
|
|
++nRemoved;
|
|
if ( n2 != n3 )
|
|
{
|
|
++nRemoved; // count quad as 2 tris for stats
|
|
}
|
|
m_Elems.FastRemove( nElem );
|
|
continue;
|
|
}
|
|
|
|
if ( n2 != n3 )
|
|
{
|
|
Vector p3 = m_Nodes[ n3 ].transform.m_vPosition;
|
|
if ( CrossProduct( p2 - p0, p3 - p0 ).Length() <= 1e-6f )
|
|
{
|
|
// make this a triangle, but save the rest of the element
|
|
++nRemoved;
|
|
elem.nNode[ 3 ] = n2;
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
if ( nRemoved )
|
|
{
|
|
Warning( "Cloth removed %d degenerate triangles, %d elements remaining. Please clean up the mesh.\n", nRemoved, m_Elems.Count() );
|
|
}
|
|
}
|
|
|
|
|
|
void CFeModelBuilder::RecomputeMasses( CUtlVector< float >& nodeMass )
|
|
{
|
|
nodeMass.SetCount( m_Nodes.Count( ) );
|
|
nodeMass.FillWithValue( 0 ); // all unaccounted nodes are going to stay static
|
|
|
|
// accumulate the automatic node masses
|
|
for ( int nElem = 0; nElem < m_Elems.Count( ); ++nElem )
|
|
{
|
|
const BuildElem_t &elem = m_Elems[ nElem ];
|
|
|
|
for ( int nTriangulate = 0; nTriangulate < 2; ++nTriangulate )
|
|
{
|
|
uint n0 = elem.nNode[ 0 ], n1 = elem.nNode[ 1 + nTriangulate ], n2 = elem.nNode[ 2 + nTriangulate ];
|
|
if ( n1 == n2 )
|
|
continue;
|
|
Assert( n0 != n1 && n1 != n2 && n2 != n0 );
|
|
Vector p0 = m_Nodes[ n0 ].transform.m_vPosition, p1 = m_Nodes[ n1 ].transform.m_vPosition, p2 = m_Nodes[ n2 ].transform.m_vPosition;
|
|
Vector vCross = CrossProduct( p1 - p0, p2 - p0 );
|
|
float flSinArea = vCross.Length( );
|
|
Assert( flSinArea >= 1e-6f );
|
|
float cot0 = DotProduct( p1 - p0, p2 - p0 ) / flSinArea;
|
|
float cot1 = DotProduct( p2 - p1, p0 - p1 ) / flSinArea;
|
|
float cot2 = DotProduct( p0 - p2, p1 - p2 ) / flSinArea;
|
|
float x01sqr = ( p1 - p0 ).LengthSqr( );
|
|
float x12sqr = ( p2 - p1 ).LengthSqr( );
|
|
float x20sqr = ( p0 - p2 ).LengthSqr( );
|
|
|
|
float m0 = 0, m1 = 0, m2 = 0;
|
|
if ( cot0 > 0 && cot1 > 0 && cot2 > 0 )
|
|
{
|
|
// a good, unobtuse triangle
|
|
m0 = .125f * ( x01sqr * cot2 + x20sqr * cot1 );
|
|
m1 = .125f * ( x01sqr * cot2 + x12sqr * cot0 );
|
|
m2 = .125f * ( x20sqr * cot1 + x12sqr * cot0 );
|
|
}
|
|
else
|
|
{
|
|
// an obtuse triangle: when it becomes right, we cross over to split 50/25/25 between obtuse/sharp/sharp
|
|
float flAqtr = .125f * flSinArea; // quarter area of triangle
|
|
m0 = m1 = m2 = flAqtr;
|
|
// the obtuse angle should get 1/2 area
|
|
if ( cot0 < cot1 )
|
|
{
|
|
if ( cot0 < cot2 )
|
|
{
|
|
Assert( cot0 <= 0 );
|
|
m0 *= 2;
|
|
}
|
|
else
|
|
{
|
|
Assert( cot2 <= 0 );
|
|
m2 *= 2;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if ( cot1 < cot2 )
|
|
{
|
|
Assert( cot1 <= 0 );
|
|
m1 *= 2;
|
|
}
|
|
else
|
|
{
|
|
Assert( cot2 <= 0 );
|
|
m2 *= 2;
|
|
}
|
|
}
|
|
}
|
|
nodeMass[ n0 ] += m0;
|
|
nodeMass[ n1 ] += m1;
|
|
nodeMass[ n2 ] += m2;
|
|
}
|
|
}
|
|
|
|
// we're assuming linear density of 8 kg/in and area density of 1 kg/in^2, and they aren't compatible,
|
|
// but it doesn't matter if we don't have the same nodes participating in both rod and quad constraints.
|
|
// if we do, the auto-mass computation will be skewed, most likely, towards the ropes rather than cloth
|
|
for ( int nRod = 0; nRod < m_Rods.Count( ); ++nRod )
|
|
{
|
|
uint n0 = m_Rods[ nRod ].nNode[ 0 ], n1 = m_Rods[ nRod ].nNode[ 1 ];
|
|
float flLength = ( m_Nodes[ n0 ].transform.m_vPosition - m_Nodes[ n1 ].transform.m_vPosition ).Length( );
|
|
nodeMass[ n0 ] += 8 * flLength;
|
|
nodeMass[ n1 ] += 8 * flLength;
|
|
}
|
|
}
|
|
|
|
// make sure that for nodes that have "global" mass multipliers, we redistributes masses so that
|
|
// the mass ratios are exactly what the user specified. The absolute values of the masses are presumed
|
|
// to not be very important and are computed automatically to balance against the distances between nodes
|
|
// note: connectivity may make this hard to tune. If we have a couple of rags of very different scales,
|
|
// the mass ratios between the "global" and "local" nodes are going to be out of whack
|
|
void CFeModelBuilder::BalanceGlobalMassMultipliers( CUtlVector< float >& nodeMass )
|
|
{
|
|
float flComputedMassSum = 0, flOverrideMassSum = 0;
|
|
for ( int i = 0; i < m_Nodes.Count( ); ++i )
|
|
{
|
|
if ( m_Nodes[ i ].bMassMultiplierGlobal && nodeMass[ i ] > 0 )
|
|
{
|
|
flComputedMassSum += nodeMass[ i ];
|
|
flOverrideMassSum += m_Nodes[ i ].flMassMultiplier;
|
|
}
|
|
}
|
|
|
|
if ( flOverrideMassSum > 0 && flComputedMassSum > 0 )
|
|
{
|
|
float flScale = flComputedMassSum / flOverrideMassSum;
|
|
for ( int i = 0; i < m_Nodes.Count( ); ++i )
|
|
{
|
|
if ( m_Nodes[ i ].bMassMultiplierGlobal && nodeMass[ i ] > 0 )
|
|
{
|
|
nodeMass[ i ] = flScale * m_Nodes[ i ].flMassMultiplier;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
bool Conflict( const FeRodConstraint_t &a, const FeRodConstraint_t &b )
|
|
{
|
|
return a.nNode[ 0 ] == b.nNode[ 0 ] || a.nNode[ 0 ] == b.nNode[ 1 ] || a.nNode[ 1 ] == b.nNode[ 0 ] || a.nNode[ 1 ] == b.nNode[ 1 ];
|
|
}
|
|
|
|
bool Conflict( const CFeModelBuilder::BuildElem_t &a, const CFeModelBuilder::BuildElem_t &b )
|
|
{
|
|
for ( int i = 0; i < 4; ++i )
|
|
{
|
|
for ( int j = 0; j < 4; ++j )
|
|
{
|
|
if ( a.nNode[ i ] == b.nNode[ j ] )
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool Conflict( const FeQuad_t &a, const FeQuad_t &b )
|
|
{
|
|
for ( int i = 0; i < 4; ++i )
|
|
{
|
|
for ( int j = 0; j < 4; ++j )
|
|
{
|
|
if ( a.nNode[ i ] == b.nNode[ j ] )
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
|
|
bool Conflict( const FeTri_t &a, const FeTri_t &b )
|
|
{
|
|
for ( int i = 0; i < 3; ++i )
|
|
{
|
|
for ( int j = 0; j < 3; ++j )
|
|
{
|
|
if ( a.nNode[ i ] == b.nNode[ j ] )
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
|
|
bool Conflict( const FeSpringIntegrator_t &a, const FeSpringIntegrator_t &b )
|
|
{
|
|
return a.nNode[ 0 ] == b.nNode[ 0 ] || a.nNode[ 0 ] == b.nNode[ 1 ] || a.nNode[ 1 ] == b.nNode[ 0 ] || a.nNode[ 1 ] == b.nNode[ 1 ];
|
|
}
|
|
|
|
|
|
void ListUsedNodes( const FeNodeBase_t &a, uint16 nNodes[ 5 ] )
|
|
{
|
|
nNodes[ 0 ] = a.nNode;
|
|
nNodes[ 1 ] = a.nNodeX0;
|
|
nNodes[ 2 ] = a.nNodeX1;
|
|
nNodes[ 3 ] = a.nNodeY0;
|
|
nNodes[ 4 ] = a.nNodeY1;
|
|
}
|
|
|
|
bool Conflict( const FeNodeBase_t &a, const FeNodeBase_t &b )
|
|
{
|
|
uint16 m[ 5 ], n[ 5 ];
|
|
ListUsedNodes( a, m );
|
|
ListUsedNodes( b, n );
|
|
for ( int i = 0; i < 5; ++i )
|
|
{
|
|
for ( int j = 0; j < 5; ++j )
|
|
{
|
|
if ( m[ i ] == n[ j ] )
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
|
|
|
|
template <typename T>
|
|
bool CanAddToSchedule( const CUtlVector< T > &schedule, const T &add, int nLanes )
|
|
{
|
|
for ( int j = ( schedule.Count( ) & ~( nLanes - 1 ) ); j < schedule.Count( ); ++j )
|
|
{
|
|
if ( Conflict( schedule[ j ], add ) )
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
template < typename T, typename TSimd >
|
|
void Schedule( CUtlVectorAligned< TSimd > *pOutput, const T *pInput, int nInputCount, int nLanes )
|
|
{
|
|
Assert( !( nLanes & ( nLanes - 1 ) ) );
|
|
CUtlVector< T > schedule, delayed;
|
|
delayed.CopyArray( pInput, nInputCount );
|
|
schedule.EnsureCapacity( delayed.Count( ) );
|
|
|
|
int nScheduleRoll = 0;
|
|
|
|
while ( delayed.Count( ) > 0 || ( schedule.Count( ) & ( nLanes - 1 ) ) )
|
|
{
|
|
bool bAdded = false;
|
|
//if ( !( i & ( nLanes - 1 ) ) )
|
|
for ( int j = delayed.Count( ); j-- > 0; )
|
|
{
|
|
if ( CanAddToSchedule( schedule, delayed[ j ], nLanes ) )
|
|
{
|
|
schedule.AddToTail( delayed[ j ] );
|
|
delayed.Remove( j );
|
|
bAdded = true;
|
|
break;
|
|
}
|
|
}
|
|
if ( !bAdded )
|
|
{
|
|
//try to find something else that we can solve twice per iteration
|
|
for ( int j = 0; j < schedule.Count( ); ++j )
|
|
{
|
|
int k = ( nScheduleRoll + j ) % schedule.Count( );
|
|
++nScheduleRoll;
|
|
if ( CanAddToSchedule( schedule, schedule[ k ], nLanes ) )
|
|
{
|
|
T duplicate = schedule[ k ];
|
|
schedule.AddToTail( duplicate );
|
|
bAdded = true;
|
|
break;
|
|
}
|
|
}
|
|
if ( !bAdded )
|
|
{
|
|
// found nothing, just duplicate the last element, it's always safe
|
|
T safeDup = schedule.Tail( );
|
|
schedule.AddToTail( safeDup );
|
|
}
|
|
}
|
|
}
|
|
|
|
Assert( !( schedule.Count( ) & ( nLanes - 1 ) ) );
|
|
pOutput->SetCount( schedule.Count( ) / nLanes );
|
|
for ( int nScalar = 0, nSimd = schedule.Count( ) / nLanes; nSimd-- > 0; nScalar += nLanes )
|
|
{
|
|
( *pOutput )[ nSimd ].Init( &schedule[ nScalar ] );
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// build the sequences of node indices from parent to child that are used in reconstruction of node rotations
|
|
void CFeModelBuilder::BuildRopes( )
|
|
{
|
|
// mark "1" for nodes that are NOT ends of ropes, e.g. they:
|
|
// a) have rope (sub)chains hanging off of them
|
|
// b) have quads attached to them, so they can form their bases by looking at those quads
|
|
// c) have preset bases for any other reason
|
|
CVarBitVec skip( m_Nodes.Count( ) );
|
|
CVarBitVec hasBase( m_Nodes.Count() );
|
|
|
|
// skip already preset base
|
|
for ( int i = 0; i < m_NodeBases.Count(); ++i )
|
|
{
|
|
uint nNode = m_NodeBases[ i ].nNode;
|
|
skip.Set( nNode );
|
|
hasBase.Set( nNode );
|
|
}
|
|
// skip free nodes
|
|
for ( int i = 0; i < m_Nodes.Count(); ++i )
|
|
{
|
|
if ( m_Nodes[ i ].bSimulated && m_Nodes[ i ].bAnimRotation )
|
|
{
|
|
skip.Set( i );
|
|
}
|
|
}
|
|
|
|
if ( IsDebug() )
|
|
{
|
|
for ( int nElem = 0; nElem < m_Elems.Count(); ++nElem )
|
|
{
|
|
const BuildElem_t &elem = m_Elems[ nElem ];
|
|
AssertDbg( elem.nNode[ 1 ] != elem.nNode[ 0 ] && elem.nNode[ 2 ] != elem.nNode[ 1 ] && elem.nNode[ 0 ] != elem.nNode[ 2 ] );
|
|
// the first 3 nodes are unique, so there's at least a triangle here (hopefully non-degenerate), so we'll try to rely on that for base generation
|
|
for ( int c = 0; c < ARRAYSIZE( elem.nNode ); ++c )
|
|
{
|
|
BuildNode_t &node = m_Nodes[ elem.nNode[ c ] ]; NOTE_UNUSED( node );
|
|
Assert( skip.IsBitSet( elem.nNode[ c ] ) || !node.bSimulated || node.bVirtual ); // should've been set when computing node bases
|
|
}
|
|
}
|
|
}
|
|
|
|
for ( int nNode = 0; nNode < m_Nodes.Count( ); ++nNode )
|
|
{
|
|
BuildNode_t &node = m_Nodes[ nNode ];
|
|
if ( !node.bSimulated || node.bVirtual )
|
|
{
|
|
// this can't be the tail of a rope
|
|
skip.Set( nNode );
|
|
hasBase.Set( nNode );
|
|
continue;
|
|
}
|
|
|
|
if ( node.nParent >= 0 )
|
|
{
|
|
// the parent can't be the tail because this node will be the tail
|
|
skip.Set( node.nParent );
|
|
}
|
|
}
|
|
|
|
// now we know which nodes are part of the chain
|
|
for ( int nTail = 0; nTail < m_Nodes.Count( ); ++nTail )
|
|
{
|
|
if ( m_Nodes[ nTail ].bSimulated && !m_Nodes[ nTail ].bVirtual && m_Nodes[ nTail ].nParent >= 0 && !skip.IsBitSet( nTail ) )
|
|
{
|
|
// starting from the tip of the chain, walk up to form a chain
|
|
CUtlVector< int > *pChain = new CUtlVector< int >;
|
|
for ( int nLink = nTail; ; nLink = m_Nodes[ nLink ].nParent )
|
|
{
|
|
if ( nLink < 0 )
|
|
{
|
|
// the chain unexpectedly terminated with a (previous) dynamic: ambiguous case, because where do we start the default transform?
|
|
AssertDbg( !pChain->IsEmpty( ) );
|
|
int nRoot = pChain->Tail( );
|
|
pChain->AddToTail( nRoot ); // just pretend the start of the chain will prime itself w.r.t. orientation.
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
pChain->AddToTail( nLink );
|
|
if ( hasBase[ nLink ] )
|
|
{
|
|
// the chain is terminated with a static: the easiest case
|
|
break;
|
|
}
|
|
skip.Set( nLink );
|
|
}
|
|
}
|
|
pChain->Reverse();
|
|
m_Ropes.AddToTail( pChain );
|
|
}
|
|
}
|
|
}
|
|
|
|
void CFeModelBuilder::BuildFreeNodes( MbaContext_t &context )
|
|
{
|
|
CVarBitVec hasBase( m_Nodes.Count() );
|
|
for ( int i = 0; i < m_Ropes.Count(); ++i )
|
|
{
|
|
for ( int j = 1; j < m_Ropes[ i ]->Count(); ++j )
|
|
{
|
|
int nNode = m_Ropes[ i ]->Element( j );
|
|
AssertDbg( !hasBase[ nNode ] );
|
|
hasBase.Set( nNode );
|
|
}
|
|
}
|
|
for ( int i = 0; i < m_NodeBases.Count(); ++i )
|
|
{
|
|
int nNode = m_NodeBases[ i ].nNode;
|
|
AssertDbg( !hasBase[ nNode ] ); //
|
|
hasBase.Set( nNode );
|
|
}
|
|
for ( int i = 0; i < context.fitMatrices.Count(); ++i )
|
|
{
|
|
const FeFitMatrix_t &fm = context.fitMatrices[ i ];
|
|
hasBase.Set( fm.nNode );
|
|
}
|
|
for ( int nNode = 0; nNode < m_Nodes.Count(); ++nNode )
|
|
{
|
|
if ( m_Nodes[ nNode ].bSimulated )
|
|
{
|
|
if ( !hasBase.IsBitSet( nNode ) || m_Nodes[ nNode ].bAnimRotation )
|
|
{
|
|
AssertDbg( !hasBase.IsBitSet( nNode ) ); // we should have node that takes rotation from animation, and also has the rotation computed by ropes or by node bases - that'd be wasteful. but otherwise, this assert is harmless.
|
|
m_FreeNodes.AddToTail( nNode );
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
template <typename Functor >
|
|
CUtlPair< int, int > FindBestPair( const CUtlSortVector< int > &neighbors, Functor fn )
|
|
{
|
|
CUtlPair< int, int > best( 0, 0 );
|
|
float flBest = -FLT_MAX;
|
|
for ( int i = 0; i < neighbors.Count( ); ++i )
|
|
{
|
|
for ( int j = i + 1; j < neighbors.Count( ); ++j )
|
|
{
|
|
float flGrade = fn( neighbors[ i ], neighbors[ j ] );
|
|
if ( flGrade >= flBest )
|
|
{
|
|
flBest = flGrade;
|
|
best.first = neighbors[ i ];
|
|
best.second = neighbors[ j ];
|
|
}
|
|
}
|
|
}
|
|
return best;
|
|
}
|
|
|
|
|
|
bool IsIdentity( const Quaternion &q, float flBiasSqr = 1e-8f )
|
|
{
|
|
return q.x * q.x + q.y * q.y + q.z * q.z < flBiasSqr;
|
|
}
|
|
|
|
|
|
FeNodeBase_t CFeModelBuilder::BuildNodeBasisFast( uint nNode )
|
|
{
|
|
return BuildNodeBasisFast( m_Nodes, nNode, *( m_NodeNeighbors[ nNode ] ) );
|
|
}
|
|
|
|
|
|
FeNodeBase_t CFeModelBuilder::BuildNodeBasisFast( const CUtlVectorAligned< BuildNode_t > &nodes, uint nNode, const CUtlSortVector< int > &neighbors )
|
|
{
|
|
FeNodeBase_t basis;
|
|
V_memset( &basis, 0, sizeof( basis ) );
|
|
basis.nNode = nNode;
|
|
|
|
// find max-length axis
|
|
CUtlPair< int, int > axisXIndices = FindBestPair( neighbors,
|
|
[&nodes] ( uint n0, uint n1 )
|
|
{
|
|
return ( nodes[ n0 ].transform.m_vPosition - nodes[ n1 ].transform.m_vPosition ).Length( );
|
|
}
|
|
);
|
|
basis.nNodeX0 = axisXIndices.second;
|
|
basis.nNodeX1 = axisXIndices.first;
|
|
|
|
Vector vAxisX = ( nodes[ basis.nNodeX1 ].transform.m_vPosition - nodes[ basis.nNodeX0 ].transform.m_vPosition );
|
|
// find max-area cross-axis
|
|
CUtlPair< int, int > axisYIndices = FindBestPair( neighbors,
|
|
[&nodes, &vAxisX] ( uint n0, uint n1 )
|
|
{
|
|
return CrossProduct( nodes[ n0 ].transform.m_vPosition - nodes[ n1 ].transform.m_vPosition, vAxisX ).Length( );
|
|
}
|
|
);
|
|
basis.nNodeY0 = axisYIndices.second;
|
|
basis.nNodeY1 = axisYIndices.first;
|
|
|
|
Vector vAxisY = ( nodes[ basis.nNodeY1 ].transform.m_vPosition - nodes[ basis.nNodeY0 ].transform.m_vPosition ).NormalizedSafe( Vector( 0,0,-1 ) );
|
|
vAxisX = ( vAxisX - DotProduct( vAxisY, vAxisX ) * vAxisY );
|
|
float flAxisXlen = vAxisX.Length( );
|
|
if ( flAxisXlen <= 0.05f )
|
|
{
|
|
CUtlString list;
|
|
for ( int j = 0; j < neighbors.Count( ); ++j )
|
|
{
|
|
if ( j )
|
|
list += ", ";
|
|
list += nodes[ neighbors[ j ] ].pName;
|
|
}
|
|
Warning( "Degenerate finite element (%s), won't be able to compute good normals for node %d bone %s\n", list.Get( ), nNode, nodes[ nNode ].pName );
|
|
Vector vAxisXAlt = CrossProduct( nodes[ nNode ].transform.m_orientation.GetUp(), vAxisY );
|
|
float flAxisXAltLen = vAxisXAlt.Length( );
|
|
if ( flAxisXAltLen < 0.001f )
|
|
{
|
|
vAxisX = VectorPerpendicularToVector( vAxisY );
|
|
}
|
|
else
|
|
{
|
|
vAxisX = vAxisXAlt / flAxisXAltLen;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
vAxisX /= flAxisXlen;
|
|
}
|
|
matrix3x4_t tmPredicted;
|
|
tmPredicted.InitXYZ( vAxisX, vAxisY, CrossProduct( vAxisX, vAxisY ), vec3_origin );
|
|
Assert( IsGoodWorldTransform( tmPredicted, 1.0f ) );
|
|
Quaternion qPredicted = MatrixQuaternion( tmPredicted );
|
|
basis.qAdjust = Conjugate( qPredicted ) * nodes[ nNode ].transform.m_orientation;
|
|
QuaternionNormalize( basis.qAdjust );
|
|
|
|
// there are a few special cases, when we can rearrange the axis order to get qAdjust to become identity
|
|
// that is more efficent to compute, we'll save a matrix multiplication per node
|
|
if ( IsIdentity( basis.qAdjust ) )
|
|
{
|
|
basis.qAdjust = quat_identity;
|
|
}
|
|
else if ( IsIdentity( basis.qAdjust * Quaternion( 0, 0, 1, 0 ) ) ) // 180 degree rotation around Z
|
|
{
|
|
// just flip X and Y axes to give adjust identity
|
|
Swap( basis.nNodeX0, basis.nNodeX1 );
|
|
Swap( basis.nNodeY0, basis.nNodeY1 );
|
|
basis.qAdjust = quat_identity;
|
|
}
|
|
else if ( IsIdentity( basis.qAdjust * Quaternion( 0, 1, 0, 0 ) ) ) // 180 degree rotation around Y
|
|
{
|
|
// just flip X axis to give adjust identity
|
|
Swap( basis.nNodeX0, basis.nNodeX1 );
|
|
basis.qAdjust = quat_identity;
|
|
}
|
|
else if ( IsIdentity( basis.qAdjust * Quaternion( 1, 0, 0, 0 ) ) ) // 180 degree rotation around X
|
|
{
|
|
// just flip X axis to give adjust identity
|
|
Swap( basis.nNodeY0, basis.nNodeY1 );
|
|
basis.qAdjust = quat_identity;
|
|
}
|
|
/* else
|
|
{ // test, debug the clauses above with this
|
|
Swap( basis.nNodeX0, basis.nNodeX1 );
|
|
Swap( basis.nNodeY0, basis.nNodeY1 );
|
|
basis.qAdjust *= Quaternion( 0, 0, 1, 0 );
|
|
}
|
|
*/
|
|
|
|
return basis;
|
|
}
|
|
|
|
|
|
void CFeModelBuilder::BuildNodeBases( )
|
|
{
|
|
BuildNodeBases( m_Nodes, m_Elems, m_PresetNodeBases, m_NodeBases, m_NodeNeighbors );
|
|
}
|
|
|
|
|
|
void CFeModelBuilder::BuildNodeBases( const CUtlVectorAligned< BuildNode_t > &nodes, const CUtlVector< BuildElem_t > &elems, const CUtlVector< FeNodeBase_t > &presetNodeBases, CUtlVector< FeNodeBase_t > &nodeBases, CUtlVectorOfPointers< CUtlSortVector< int > > &neighbors )
|
|
{
|
|
neighbors.SetCountAndInit( nodes.Count( ),
|
|
[] ( int nNode )
|
|
{
|
|
CUtlSortVector< int > *pResult = new CUtlSortVector< int >;
|
|
pResult->Insert( nNode ); // node is always a neighbor of itself..
|
|
return pResult;
|
|
}
|
|
);
|
|
|
|
for ( int nElem = 0; nElem < elems.Count( ); ++nElem )
|
|
{
|
|
const BuildElem_t &elem = elems[ nElem ];
|
|
AssertDbg( elem.nNode[ 1 ] != elem.nNode[ 0 ] && elem.nNode[ 2 ] != elem.nNode[ 1 ] && elem.nNode[ 0 ] != elem.nNode[ 2 ] );
|
|
int numNodes = elem.nNode[ 3 ] == elem.nNode[ 2 ] ? 3 : 4;
|
|
for ( int c = 0; c < numNodes; ++c )
|
|
{
|
|
for ( int d = 0; d < numNodes; ++d )
|
|
{
|
|
// add the previous and the next node
|
|
neighbors[ elem.nNode[ c ] ]->InsertIfNotFound( elem.nNode[ d ] );
|
|
}
|
|
}
|
|
}
|
|
|
|
// copy the preset node bases
|
|
nodeBases.EnsureCapacity( presetNodeBases.Count() );
|
|
CVarBitVec nodeWithBase( nodes.Count() );
|
|
for ( int i = 0; i < presetNodeBases.Count(); ++i )
|
|
{
|
|
const FeNodeBase_t &nb = presetNodeBases[ i ];
|
|
if ( !nodeWithBase.IsBitSet( nb.nNode ) )
|
|
{
|
|
nodeBases.AddToTail( nb );
|
|
nodeWithBase.Set( nb.nNode );
|
|
}
|
|
}
|
|
|
|
|
|
// compute additional bases
|
|
for ( int nNode = 0; nNode < nodes.Count( ); ++nNode )
|
|
{
|
|
if ( !nodeWithBase.IsBitSet( nNode ) && !( nodes[ nNode ].bSimulated && nodes[ nNode ].bAnimRotation ) )
|
|
{
|
|
CUtlSortVector< int > *pCheck = neighbors[ nNode ];
|
|
const BuildNode_t &node = nodes[ nNode ];
|
|
if ( pCheck->Count() >= 3 // we need at least 3 nodes to compute the basis
|
|
&& ( !node.bVirtual || node.bNeedNodeBase ) // virtual nodes have no effect on animation, there's no need to do a complex computation of basis for them
|
|
&& ( node.bSimulated || node.bFreeRotation ) // static nodes need not recompute their basis, they get it from animation; unless we explicitly specified we need a free-rotating locked/static/keyframed node. Then compute the rotation.
|
|
)
|
|
{
|
|
nodeBases.AddToTail( BuildNodeBasisFast( nodes, nNode, *( neighbors[ nNode ] ) ) );
|
|
nodeWithBase.Set( nNode );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
int RopeIndexCount( const CUtlVectorOfPointers< CUtlVector< int > > &ropes )
|
|
{
|
|
int nCount = ropes.Count( );
|
|
for ( int i = 0; i < ropes.Count( ); ++i )
|
|
{
|
|
nCount += ropes[ i ]->Count( );
|
|
}
|
|
return nCount;
|
|
}
|
|
|
|
|
|
void CFeModelBuilder::ValidateBases( )
|
|
{
|
|
#ifdef DBGFLAG_ASSERT
|
|
int nLog = CommandLine( )->FindParm( "-log" );
|
|
|
|
CVarBitVec hasBase( m_Nodes.Count( ) );
|
|
for ( int i = 0; i < m_Ropes.Count( ); ++i )
|
|
{
|
|
for ( int j = 1; j < m_Ropes[ i ]->Count( ); ++j )
|
|
{
|
|
int nNode = m_Ropes[ i ]->Element( j );
|
|
AssertDbg( !hasBase[ nNode ] );
|
|
hasBase.Set( nNode );
|
|
}
|
|
}
|
|
for ( int i = 0; i < m_NodeBases.Count( ); ++i )
|
|
{
|
|
int nNode = m_NodeBases[ i ].nNode;
|
|
AssertDbg( !hasBase[ nNode ] ); //
|
|
hasBase.Set( nNode );
|
|
}
|
|
for ( int i = 0; i < m_FreeNodes.Count(); ++i )
|
|
{
|
|
hasBase.Set( m_FreeNodes[ i ] );
|
|
}
|
|
|
|
//--
|
|
for ( int nNode = 0; nNode < m_Nodes.Count( ); ++nNode )
|
|
{
|
|
int nHasBase = hasBase[ nNode ] ? 1 : 0; NOTE_UNUSED( nHasBase );
|
|
int nSimulated = m_Nodes[ nNode ].bSimulated ? 1 : 0; NOTE_UNUSED( nSimulated );
|
|
//const char *pBoneName = m_Nodes[ nNode ].pName; NOTE_UNUSED( pBoneName );
|
|
Assert( nHasBase == nSimulated || m_Nodes[ nNode ].bVirtual || m_Nodes[ nNode ].bFreeRotation ); // we don't really care if virtual nodes have bases or not. In fact, it's probably better not to compute bases for them, but it may be necessary if real nodes' bases depend on them..
|
|
if ( m_Nodes[ nNode ].nParent < 0 )
|
|
{
|
|
if ( nLog )
|
|
{
|
|
PrintNodeTree( nNode, "" );
|
|
}
|
|
}
|
|
}
|
|
|
|
//
|
|
// Just print out the elements for eyeballing
|
|
//
|
|
for ( int nElem = 0; nElem < m_Elems.Count( ); ++nElem )
|
|
{
|
|
CUtlVectorFixedGrowable< const char *, 4 > names;
|
|
const BuildElem_t &elem = m_Elems[ nElem ];
|
|
int nPrefixLen = 1000;
|
|
for ( uint n = 0; n < elem.NumNodes( ); ++n )
|
|
{
|
|
const char *pName = m_Nodes[ elem.nNode[ n ] ].pName;
|
|
names.AddToTail( pName );
|
|
int nNewLen = 0;
|
|
for ( ; nNewLen < nPrefixLen && pName && pName[ nNewLen ] && names[ 0 ][ nNewLen ] == pName[ nNewLen ]; ++nNewLen )
|
|
continue;
|
|
nPrefixLen = nNewLen;
|
|
}
|
|
CUtlString list;
|
|
list.SetDirect( names[ 0 ], nPrefixLen );
|
|
list += "( ";
|
|
for ( int n = 0; n < names.Count( ); ++n )
|
|
{
|
|
if ( n )
|
|
list += ", ";
|
|
list += names[ n ] ? names[ n ] + nPrefixLen : "null";
|
|
}
|
|
list += " )";
|
|
if ( nLog )
|
|
{
|
|
Msg( "%s\n", list.Get( ) );
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
|
|
void CFeModelBuilder::PrintNodeTree( uint nRoot, const CUtlString &prefix )
|
|
{
|
|
const BuildNode_t &root = m_Nodes[ nRoot ];
|
|
Msg( "%s%s%s #%d", prefix.Get( ), root.pName, root.bSimulated ? " sim" : "", nRoot );
|
|
if ( root.flFollowWeight > 0 && root.nFollowParent >= 0 )
|
|
{
|
|
Msg( " - follow %s w%.3f", m_Nodes[ root.nFollowParent ].pName, root.flFollowWeight );
|
|
}
|
|
Msg( "\n" );
|
|
for ( int nChild = 0; nChild < m_Nodes.Count( ); ++nChild )
|
|
{
|
|
if ( m_Nodes[ nChild ].nParent == int( nRoot ) )
|
|
{
|
|
PrintNodeTree( nChild, prefix + " " );
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void CFeModelBuilder::CheckIdentityCtrlOrder( )
|
|
{
|
|
if ( m_bIdentityCtrlOrder )
|
|
{
|
|
// convert all Ctrl-based indices to node-based indices
|
|
for ( int i = 0; i < m_CtrlOffsets.Count(); ++i )
|
|
{
|
|
FeCtrlOffset_t &offset = m_CtrlOffsets[ i ];
|
|
ConvertCtrlToNode( offset.nCtrlChild );
|
|
ConvertCtrlToNode( offset.nCtrlParent );
|
|
}
|
|
|
|
for ( int i = 0; i < m_CtrlOsOffsets.Count(); ++i )
|
|
{
|
|
FeCtrlOsOffset_t &offset = m_CtrlOsOffsets[ i ];
|
|
ConvertCtrlToNode( offset.nCtrlChild );
|
|
ConvertCtrlToNode( offset.nCtrlParent );
|
|
}
|
|
|
|
for ( int i = 0; i < m_ReverseOffsets.Count(); ++i )
|
|
{
|
|
FeNodeReverseOffset_t &revOffset = m_ReverseOffsets[ i ];
|
|
ConvertCtrlToNode( revOffset.nBoneCtrl );
|
|
}
|
|
|
|
m_NodeToCtrl.SetCount( m_Nodes.Count( ) );
|
|
m_CtrlToNode.SetCount( m_Nodes.Count( ) );
|
|
for ( int i = 0; i < m_Nodes.Count( ); ++i )
|
|
{
|
|
m_NodeToCtrl[ i ] = i;
|
|
m_CtrlToNode[ i ] = i;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if ( m_NodeToCtrl.Count( ) == m_Nodes.Count( ) && m_CtrlToNode.Count( ) == m_Nodes.Count( ) )
|
|
{
|
|
for ( int i = 0; i < m_Nodes.Count( ); ++i )
|
|
{
|
|
if ( m_NodeToCtrl[ i ] != i || m_CtrlToNode[ i ] != i )
|
|
return;
|
|
}
|
|
// we already have de facto identity order
|
|
m_bIdentityCtrlOrder = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
bool CFeModelBuilder::HasLegacyStretchForce( ) const
|
|
{
|
|
for ( int i = 0; i < m_Nodes.Count( ); ++i )
|
|
{
|
|
if ( m_Nodes[ i ].bSimulated && m_Nodes[ i ].flLegacyStretchForce != 0 )
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
|
|
|
|
template <typename Allocator >
|
|
void CFeModelBuilder::OnAllocateMultiBuffer( Allocator &subAlloc, MbaContext_t &context )
|
|
{
|
|
uint nDynCount = GetDynamicNodeCount();
|
|
subAlloc( m_pSimdRods, context.simdRods );
|
|
if ( subAlloc( m_pSimdQuads, m_nSimdQuadCount[ 0 ] ) )
|
|
{
|
|
for ( uint nSimdQuad = 0; nSimdQuad < m_nSimdQuadCount[ 2 ]; ++nSimdQuad )
|
|
{
|
|
m_pSimdQuads[ nSimdQuad ] = context.simdQuads[ 2 ][ nSimdQuad ];
|
|
}
|
|
for ( uint nSimdQuad = m_nSimdQuadCount[ 2 ]; nSimdQuad < m_nSimdQuadCount[ 1 ]; ++nSimdQuad )
|
|
{
|
|
m_pSimdQuads[ nSimdQuad ] = context.simdQuads[ 1 ][ nSimdQuad - m_nSimdQuadCount[ 2 ] ];
|
|
}
|
|
for ( uint nSimdQuad = m_nSimdQuadCount[ 1 ]; nSimdQuad < m_nSimdQuadCount[ 0 ]; ++nSimdQuad )
|
|
{
|
|
m_pSimdQuads[ nSimdQuad ] = context.simdQuads[ 0 ][ nSimdQuad - m_nSimdQuadCount[ 1 ] ];
|
|
}
|
|
}
|
|
if ( subAlloc( m_pSimdTris, m_nSimdTriCount[ 0 ] ) )
|
|
{
|
|
for ( uint nSimdTri = 0; nSimdTri < m_nSimdTriCount[ 2 ]; ++nSimdTri )
|
|
{
|
|
m_pSimdTris[ nSimdTri ] = context.simdTris[ 2 ][ nSimdTri ];
|
|
}
|
|
for ( uint nSimdTri = m_nSimdTriCount[ 2 ]; nSimdTri < m_nSimdTriCount[ 1 ]; ++nSimdTri )
|
|
{
|
|
m_pSimdTris[ nSimdTri ] = context.simdTris[ 1 ][ nSimdTri - m_nSimdTriCount[ 2 ] ];
|
|
}
|
|
for ( uint nSimdTri = m_nSimdTriCount[ 1 ]; nSimdTri < m_nSimdTriCount[ 0 ]; ++nSimdTri )
|
|
{
|
|
m_pSimdTris[ nSimdTri ] = context.simdTris[ 0 ][ nSimdTri - m_nSimdTriCount[ 1 ] ];
|
|
}
|
|
}
|
|
subAlloc( m_pSimdNodeBases, context.simdBases ); // must be aligned
|
|
subAlloc( m_pSimdSpringIntegrator, context.simdSprings );
|
|
subAlloc( m_pFitMatrices, context.fitMatrices );
|
|
if ( subAlloc( m_pInitPose, m_nCtrlCount ) )
|
|
{
|
|
for ( uint nCtrl = 0; nCtrl < m_nCtrlCount; ++nCtrl )
|
|
{
|
|
m_pInitPose[ nCtrl ] = m_CtrlToNode[ nCtrl ] >= 0 ? m_Nodes[ m_CtrlToNode[ nCtrl ] ].transform : g_TransformIdentity;
|
|
}
|
|
}
|
|
subAlloc( m_pCtrlName, context.nCtrlNameCount );
|
|
subAlloc( m_pNodeBases, m_NodeBases );
|
|
subAlloc( m_pReverseOffsets, m_ReverseOffsets );
|
|
subAlloc( m_pCtrlOffsets, m_CtrlOffsets );
|
|
subAlloc( m_pCtrlOsOffsets, m_CtrlOsOffsets );
|
|
subAlloc( m_pFollowNodes, context.nodeFollowers );
|
|
uint32 *pCtrlHash = subAlloc( m_pCtrlHash, context.nCtrlNameCount );
|
|
subAlloc( m_pQuads, context.quads );
|
|
subAlloc( m_pRods, m_Rods );
|
|
subAlloc( m_pTris, context.tris );
|
|
|
|
subAlloc( m_pAxialEdges, m_AxialEdges );
|
|
if ( subAlloc( m_pNodeIntegrator, context.nNodeIntegratorCount ) )
|
|
{
|
|
for ( uint nNode = 0; nNode < context.nNodeIntegratorCount; ++nNode )
|
|
{
|
|
m_pNodeIntegrator[ nNode ] = m_Nodes[ nNode ].integrator;
|
|
if ( nNode < m_nStaticNodes )
|
|
{
|
|
m_nStaticNodeFlags |= m_Nodes[ nNode ].GetDampingFlags( );
|
|
// note: flAnimationForceAttraction seemingly isn't used in source1 on static nodes, no need to premultiply
|
|
}
|
|
else
|
|
{
|
|
// <sergiy> Important fix: the springs (particularly animation force attraction) in Source1 were tuned to the "mass" of cloth particles
|
|
// Source2 computes accelerations directly, so we need to pre-divide by mass here. I forgot to do that in CL 2322760, fixed in 2532503.
|
|
// <sergiy> turns out I do predivide flAnimationForceAttraction by mass when importing CAuthClothParser::ParseLegacyDotaNodeGrid, so no need to do that here again.
|
|
// m_pNodeIntegrator[ nNode ].flAnimationForceAttraction *= m_Nodes[ nNode ].invMass;
|
|
m_nDynamicNodeFlags |= m_Nodes[ nNode ].GetDampingFlags();
|
|
}
|
|
}
|
|
}
|
|
subAlloc( m_pSpringIntegrator, context.springs );
|
|
if ( subAlloc( m_pNodeInvMasses, m_nNodeCount ) )
|
|
{
|
|
for ( int nNode = 0; nNode < m_Nodes.Count( ); ++nNode )
|
|
{
|
|
m_pNodeInvMasses[ nNode ] = m_Nodes[ nNode ].invMass;
|
|
}
|
|
}
|
|
|
|
subAlloc( m_pCollisionSpheres, context.collisionSpheres );
|
|
subAlloc( m_pCollisionPlanes, context.collisionPlanes );
|
|
|
|
subAlloc( m_pFitWeights, context.fitWeights );
|
|
|
|
if ( context.m_bHasNodeCollisionRadii )
|
|
{
|
|
if ( subAlloc( m_pNodeCollisionRadii, GetDynamicNodeCount() ) )
|
|
{
|
|
AssertDbg( m_Nodes.Count() == int( m_nNodeCount ) );
|
|
for ( uint nNode = m_nStaticNodes; nNode < m_nNodeCount; ++nNode )
|
|
{
|
|
m_pNodeCollisionRadii[ nNode - m_nStaticNodes ] = m_Nodes[ nNode ].flCollisionRadius;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
m_pNodeCollisionRadii = NULL;
|
|
}
|
|
|
|
if ( context.m_bUsePerNodeLocalForce )
|
|
{
|
|
if ( subAlloc( m_pLocalForce, GetDynamicNodeCount() ) )
|
|
{
|
|
AssertDbg( m_Nodes.Count() == int( m_nNodeCount ) );
|
|
for ( uint nNode = m_nStaticNodes; nNode < m_nNodeCount; ++nNode )
|
|
{
|
|
m_pLocalForce[ nNode - m_nStaticNodes ] = m_Nodes[ nNode ].flLocalForce;
|
|
}
|
|
}
|
|
}
|
|
if ( context.m_bUsePerNodeLocalRotation )
|
|
{
|
|
if ( subAlloc( m_pLocalRotation, GetDynamicNodeCount() ) )
|
|
{
|
|
AssertDbg( m_Nodes.Count() == int( m_nNodeCount ) );
|
|
for ( uint nNode = m_nStaticNodes; nNode < m_nNodeCount; ++nNode )
|
|
{
|
|
m_pLocalRotation[ nNode - m_nStaticNodes ] = m_Nodes[ nNode ].flLocalRotation;
|
|
}
|
|
}
|
|
}
|
|
|
|
subAlloc( m_pWorldCollisionParams, context.worldCollisionParams );
|
|
subAlloc( m_pTaperedCapsuleStretches, m_TaperedCapsuleStretches );
|
|
subAlloc( m_pTaperedCapsuleRigids, m_TaperedCapsuleRigids );
|
|
subAlloc( m_pSphereRigids, m_SphereRigids );
|
|
|
|
if ( subAlloc( m_pLegacyStretchForce, context.nLegacyStretchForceCount ) )
|
|
{
|
|
for ( uint i = 0; i < context.nLegacyStretchForceCount; ++i )
|
|
{
|
|
m_pLegacyStretchForce[ i ] = m_Nodes[ i ].bSimulated ? m_Nodes[ i ].flLegacyStretchForce/* * m_Nodes[ i ].invMass*/ : 0.0f;
|
|
}
|
|
}
|
|
|
|
if ( !m_bIdentityCtrlOrder )
|
|
{
|
|
if ( subAlloc( m_pCtrlToNode, m_CtrlToNode.Count( ) ) )
|
|
{
|
|
for ( int i = 0; i < m_CtrlToNode.Count( ); ++i )
|
|
{
|
|
m_pCtrlToNode[ i ] = m_CtrlToNode[ i ];
|
|
}
|
|
}
|
|
if ( subAlloc( m_pNodeToCtrl, m_NodeToCtrl.Count( ) ) )
|
|
{
|
|
for ( int i = 0; i < m_NodeToCtrl.Count( ); ++i )
|
|
{
|
|
m_pNodeToCtrl[ i ] = m_NodeToCtrl[ i ];
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// these get assigned NULL twice..
|
|
m_pCtrlToNode = NULL;
|
|
m_pNodeToCtrl = NULL;
|
|
}
|
|
|
|
subAlloc( m_pTreeChildren, m_TreeChildren );
|
|
subAlloc( m_pTreeParents, m_TreeParents );
|
|
if ( subAlloc( m_pTreeCollisionMasks, m_TreeParents.Count() ) )
|
|
{
|
|
V_memset( m_pTreeCollisionMasks, 0, sizeof( *m_pTreeCollisionMasks ) * ( 2 * nDynCount - 1 ) );
|
|
for ( uint i = 0; i < nDynCount; ++i )
|
|
{
|
|
m_pTreeCollisionMasks[ i ] = m_Nodes[ i + m_nStaticNodes ].nCollisionMask;
|
|
}
|
|
// propagate the mask to parents
|
|
for ( uint i = 0; i < 2 * nDynCount - 2; ++i )
|
|
{
|
|
m_pTreeCollisionMasks[ m_TreeParents[ i ] ] |= m_pTreeCollisionMasks[ i ];
|
|
}
|
|
}
|
|
|
|
subAlloc( m_pWorldCollisionNodes, context.worldCollisionNodes );
|
|
subAlloc( m_pFreeNodes, m_FreeNodes );
|
|
|
|
if ( subAlloc( m_pRopes, m_nRopeIndexCount ) )
|
|
{
|
|
uint nRopeRunningIndex = m_Ropes.Count( );
|
|
for ( int nRope = 0; nRope < m_Ropes.Count( ); ++nRope )
|
|
{
|
|
for ( int nLink = 0; nLink < m_Ropes[ nRope ]->Count( ); ++nLink )
|
|
{
|
|
m_pRopes[ nRopeRunningIndex++ ] = m_Ropes[ nRope ]->Element( nLink );
|
|
}
|
|
m_pRopes[ nRope ] = nRopeRunningIndex;
|
|
}
|
|
Assert( nRopeRunningIndex == m_nRopeIndexCount );
|
|
}
|
|
|
|
for ( uint nCtrl = 0; nCtrl < context.nCtrlNameCount; ++nCtrl )
|
|
{
|
|
int nNode = m_CtrlToNode[ nCtrl ];
|
|
subAlloc.String( m_pCtrlName[ nCtrl ], nNode >= 0 ? m_Nodes[ nNode ].pName : NULL );
|
|
}
|
|
|
|
if ( pCtrlHash )
|
|
{
|
|
for ( uint nCtrl = 0; nCtrl < context.nCtrlNameCount; ++nCtrl )
|
|
{
|
|
const char *pName = m_pCtrlName[ nCtrl ];
|
|
pCtrlHash[ nCtrl ] = pName ? MakeStringToken( pName ).m_nHashCode : 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
float CFeModelBuilder::ElemNormalLength( const uint nNode[ 4 ] )
|
|
{
|
|
Vector p0 = m_Nodes[ nNode[ 0 ] ].transform.m_vPosition;
|
|
Vector p1 = m_Nodes[ nNode[ 1 ] ].transform.m_vPosition;
|
|
Vector p2 = m_Nodes[ nNode[ 2 ] ].transform.m_vPosition;
|
|
Vector p3 = m_Nodes[ nNode[ 3 ] ].transform.m_vPosition;
|
|
Vector vNormal = CrossProduct( p2 - p0, p3 - p1 );
|
|
return vNormal.Length();
|
|
}
|
|
|
|
Vector CFeModelBuilder::TriNormal( uint nNode0, uint nNode1, uint nNode2 )
|
|
{
|
|
Vector p0 = m_Nodes[ nNode0 ].transform.m_vPosition;
|
|
Vector p1 = m_Nodes[ nNode1 ].transform.m_vPosition;
|
|
Vector p2 = m_Nodes[ nNode2 ].transform.m_vPosition;
|
|
|
|
Vector vNormal = CrossProduct( p1 - p0, p2 - p0 );
|
|
return vNormal;
|
|
}
|
|
|
|
float CFeModelBuilder::NodeDist( uint nNode0, uint nNode1 )
|
|
{
|
|
const Vector &p0 = m_Nodes[ nNode0 ].transform.m_vPosition;
|
|
const Vector &p1 = m_Nodes[ nNode1 ].transform.m_vPosition;
|
|
return ( p0 - p1 ).Length();
|
|
}
|
|
|
|
|
|
void CFeModelBuilder::AdjustQuads()
|
|
{
|
|
CUtlVector< BuildElem_t > append;
|
|
// convert self-intersecting quads to nice-looking quads
|
|
for ( int i = 0; i < m_Elems.Count(); ++i )
|
|
{
|
|
BuildElem_t &elem = m_Elems[ i ];
|
|
if ( elem.NumNodes() < 4 )
|
|
{
|
|
// nothing to adjust
|
|
continue;
|
|
}
|
|
|
|
switch( elem.nStaticNodes )
|
|
{
|
|
case 2:
|
|
{ // just a nicety: if we can, let's make this into a convex quad. it's not necessary (and impossible in case of diagonal statics)
|
|
Vector e0 = m_Nodes[ elem.nNode[ 1 ] ].transform.m_vPosition - m_Nodes[ elem.nNode[ 0 ] ].transform.m_vPosition;
|
|
Vector e2 = m_Nodes[ elem.nNode[ 2 ] ].transform.m_vPosition - m_Nodes[ elem.nNode[ 3 ] ].transform.m_vPosition;
|
|
if ( DotProduct( e2, e0 ) < 0 )
|
|
{
|
|
Swap( elem.nNode[ 3 ], elem.nNode[ 2 ] );
|
|
}
|
|
}
|
|
break;
|
|
|
|
case 1:
|
|
case 0:
|
|
{
|
|
float flNoPermValue = ElemNormalLength( elem.nNode );
|
|
uint nPerm1[ 4 ] = { elem.nNode[ 0 ], elem.nNode[ 2 ], elem.nNode[ 3 ], elem.nNode[ 1 ] };
|
|
uint nPerm2[ 4 ] = { elem.nNode[ 0 ], elem.nNode[ 3 ], elem.nNode[ 1 ], elem.nNode[ 2 ] };
|
|
float flPerm1Value = ElemNormalLength( nPerm1 );
|
|
float flPerm2Value = ElemNormalLength( nPerm2 );
|
|
uint *pBestPerm = NULL;
|
|
if ( flPerm1Value > flPerm2Value )
|
|
{
|
|
if ( flPerm1Value > flNoPermValue )
|
|
{
|
|
pBestPerm = nPerm1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if ( flPerm2Value > flNoPermValue )
|
|
{
|
|
pBestPerm = nPerm2;
|
|
}
|
|
}
|
|
if ( pBestPerm )
|
|
{
|
|
elem.nNode[ 0 ] = pBestPerm[ 0 ];
|
|
elem.nNode[ 1 ] = pBestPerm[ 1 ];
|
|
elem.nNode[ 2 ] = pBestPerm[ 2 ];
|
|
elem.nNode[ 3 ] = pBestPerm[ 3 ];
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
if ( m_flQuadBendTolerance < 1.0f && elem.nStaticNodes == 0 && elem.nNode[ 2 ] != elem.nNode[ 3 ] )
|
|
{
|
|
// check if the quad is bent too much, and triangulate if so
|
|
if ( NodeDist( elem.nNode[ 0 ], elem.nNode[ 2 ] ) > NodeDist( elem.nNode[ 1 ], elem.nNode[ 3 ] ) )
|
|
{
|
|
// try to make it so that triangulation (0,1,2) (2,3,0) produces the shortest diagonal (0,2)
|
|
uint nWasNode0 = elem.nNode[ 0 ];
|
|
elem.nNode[ 0 ] = elem.nNode[ 1 ];
|
|
elem.nNode[ 1 ] = elem.nNode[ 2 ];
|
|
elem.nNode[ 2 ] = elem.nNode[ 3 ];
|
|
elem.nNode[ 3 ] = nWasNode0;
|
|
}
|
|
|
|
// how much is it bent? compare sine of the bend angle with the limit
|
|
Vector n0 = TriNormal( elem.nNode[ 0 ], elem.nNode[ 1 ], elem.nNode[ 2 ] );
|
|
Vector n1 = TriNormal( elem.nNode[ 2 ], elem.nNode[ 3 ], elem.nNode[ 0 ] );
|
|
float flSinBendF = CrossProduct( n0, n1 ).Length(), flF = n0.Length() * n1.Length();
|
|
if ( flSinBendF > m_flQuadBendTolerance * flF )
|
|
{
|
|
// the bend is too great. Split the quad
|
|
BuildElem_t newElem = elem; // new element is (0,2,3,3)
|
|
newElem.nNode[ 1 ] = newElem.nNode[ 2 ];
|
|
newElem.nNode[ 2 ] = newElem.nNode[ 3 ];
|
|
append.AddToTail( newElem );
|
|
|
|
// convert old elem to (0,1,2,2) triangle
|
|
elem.nNode[ 3 ] = elem.nNode[ 2 ];
|
|
}
|
|
}
|
|
}
|
|
|
|
m_Elems.AddMultipleToTail( append.Count(), append.Base() );
|
|
}
|
|
|
|
|
|
bool CFeModelBuilder::Finish( bool bTriangulate, float flAddCurvature, float flSlackMultiplier )
|
|
{
|
|
//Schedule( , m_Elems.Base(), 4 ); NOTE_UNUSED( nElemSlack );
|
|
MbaContext_t context;
|
|
|
|
BuildInvMassesAndSortNodes( );
|
|
m_nNodeCount = m_Nodes.Count();
|
|
m_nCtrlCount = m_CtrlToNode.Count();
|
|
|
|
AdjustQuads();
|
|
|
|
BuildTree();
|
|
CheckIdentityCtrlOrder( );
|
|
BuildNodeBases( ); // all node bases are built; even those we won't need later
|
|
|
|
BuildRopes();
|
|
ValidateBases( );
|
|
|
|
BuildFeEdgeDesc( );
|
|
BuildNodeSlack( flSlackMultiplier );
|
|
BuildAndSortRods( clamp( flAddCurvature, 0.0f, 1.0f ) * M_PI, bTriangulate );
|
|
|
|
//BuildKelagerBends();
|
|
if ( m_bRigidEdgeHinges )
|
|
{
|
|
BuildAxialEdges();
|
|
}
|
|
|
|
RemoveFullyStaticElems(); // we don't need fully static quads anymore
|
|
|
|
if ( bTriangulate )
|
|
{
|
|
BuildTris( context.tris, true );
|
|
}
|
|
else
|
|
{
|
|
BuildTris( context.tris, false );
|
|
BuildQuads( context.quads, true );
|
|
}
|
|
|
|
Schedule( &context.simdRods, m_Rods.Base( ), m_Rods.Count( ), 4 );
|
|
|
|
Schedule( &context.simdQuads[ 0 ], context.quads.Base( ) + m_nQuadCount[ 1 ], m_nQuadCount[ 0 ] - m_nQuadCount[ 1 ], 4 );
|
|
Schedule( &context.simdQuads[ 1 ], context.quads.Base( ) + m_nQuadCount[ 2 ], m_nQuadCount[ 1 ] - m_nQuadCount[ 2 ], 4 );
|
|
Schedule( &context.simdQuads[ 2 ], context.quads.Base( ), m_nQuadCount[ 2 ], 4 );
|
|
m_nSimdQuadCount[ 0 ] = context.simdQuads[ 2 ].Count( ) + context.simdQuads[ 1 ].Count( ) + context.simdQuads[ 0 ].Count( );
|
|
m_nSimdQuadCount[ 1 ] = context.simdQuads[ 2 ].Count( ) + context.simdQuads[ 1 ].Count( );
|
|
m_nSimdQuadCount[ 2 ] = context.simdQuads[ 2 ].Count( );
|
|
|
|
Schedule( &context.simdTris[ 0 ], context.tris.Base( ) + m_nTriCount[ 1 ], m_nTriCount[ 0 ] - m_nTriCount[ 1 ], 4 );
|
|
Schedule( &context.simdTris[ 1 ], context.tris.Base( ) + m_nTriCount[ 2 ], m_nTriCount[ 1 ] - m_nTriCount[ 2 ], 4 );
|
|
Schedule( &context.simdTris[ 2 ], context.tris.Base( ), m_nTriCount[ 2 ], 4 );
|
|
m_nSimdTriCount[ 0 ] = context.simdTris[ 2 ].Count( ) + context.simdTris[ 1 ].Count( ) + context.simdTris[ 0 ].Count( );
|
|
m_nSimdTriCount[ 1 ] = context.simdTris[ 2 ].Count( ) + context.simdTris[ 1 ].Count( );
|
|
m_nSimdTriCount[ 2 ] = context.simdTris[ 2 ].Count( );
|
|
|
|
Schedule( &context.simdBases, m_NodeBases.Base( ), m_NodeBases.Count( ), 4 );
|
|
|
|
BuildSprings( context.springs );
|
|
Schedule( &context.simdSprings, context.springs.Base( ), context.springs.Count( ), 4 );
|
|
|
|
BuildCtrlOffsets( );
|
|
|
|
BuildNodeFollowers( context.nodeFollowers );
|
|
|
|
context.nCollisionEllipsoidsInclusive = BuildCollisionSpheres( context.collisionSpheres );
|
|
BuildCollisionPlanes( context.collisionPlanes );
|
|
|
|
BuildWorldCollisionNodes( context.worldCollisionParams, context.worldCollisionNodes );
|
|
BuildFitMatrices( context );
|
|
if ( m_bNeedBacksolvedBasesOnly )
|
|
RemoveStandaloneNodeBases( context );
|
|
BuildFreeNodes( context );
|
|
|
|
if ( HasLegacyStretchForce( ) )
|
|
{
|
|
context.nLegacyStretchForceCount = m_Nodes.Count();
|
|
}
|
|
else
|
|
{
|
|
context.nLegacyStretchForceCount = 0;
|
|
}
|
|
|
|
context.m_bHasNodeCollisionRadii = false;
|
|
for ( uint i = m_nStaticNodes; i < m_nNodeCount; ++i )
|
|
{
|
|
const BuildNode_t &node = m_Nodes[ i ];
|
|
if ( node.flCollisionRadius != 0 )
|
|
{
|
|
context.m_bHasNodeCollisionRadii = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
context.m_bUsePerNodeLocalRotation = false;
|
|
context.m_bUsePerNodeLocalForce = false;
|
|
if ( m_bUsePerNodeLocalForceAndRotation )
|
|
{
|
|
if ( m_nNodeCount > m_nStaticNodes )
|
|
{
|
|
m_flLocalForce = m_Nodes[ m_nStaticNodes ].flLocalForce; // find the max local force/rotation values and stick them into defaults
|
|
m_flLocalRotation = m_Nodes[ m_nStaticNodes ].flLocalRotation;
|
|
for ( uint i = m_nStaticNodes + 1; i < m_nNodeCount; ++i )
|
|
{
|
|
const BuildNode_t &node = m_Nodes[ i ];
|
|
if ( node.flLocalRotation != m_flLocalRotation )
|
|
{
|
|
context.m_bUsePerNodeLocalRotation = true;
|
|
m_flLocalRotation = Max( m_flLocalRotation, node.flLocalRotation );
|
|
}
|
|
if ( node.flLocalForce != m_flLocalForce )
|
|
{
|
|
context.m_bUsePerNodeLocalForce = true;
|
|
m_flLocalForce = Max( m_flLocalForce, node.flLocalForce );
|
|
}
|
|
}
|
|
|
|
if ( !( m_flLocalForce > 0 ) )
|
|
{
|
|
m_flLocalForce = 0;
|
|
context.m_bUsePerNodeLocalForce = false;
|
|
}
|
|
if ( !( m_flLocalRotation > 0 ) )
|
|
{
|
|
m_flLocalRotation = 0;
|
|
context.m_bUsePerNodeLocalRotation = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
// m_nQuadCount[0] = m_Elems.Count( ); // already assigned in BuildQuads
|
|
m_nRodCount = m_Rods.Count( );
|
|
m_nSimdRodCount = context.simdRods.Count( );
|
|
m_nAxialEdgeCount = m_AxialEdges.Count( );
|
|
m_nRopeCount = m_Ropes.Count( );
|
|
m_nRopeIndexCount = RopeIndexCount( m_Ropes );
|
|
m_nNodeBaseCount = m_NodeBases.Count( );
|
|
m_nSimdNodeBaseCount = context.simdBases.Count( );
|
|
m_nSpringIntegratorCount = context.springs.Count( );
|
|
m_nSimdSpringIntegratorCount = context.simdSprings.Count( );
|
|
m_nCtrlOffsets = m_CtrlOffsets.Count();
|
|
m_nCtrlOsOffsets = m_CtrlOsOffsets.Count( );
|
|
m_nFollowNodeCount = context.nodeFollowers.Count( );
|
|
m_nCollisionPlanes = context.collisionPlanes.Count( );
|
|
m_nCollisionSpheres[ 0 ] = context.collisionSpheres.Count( );
|
|
m_nCollisionSpheres[ 1 ] = context.nCollisionEllipsoidsInclusive;
|
|
m_nWorldCollisionNodeCount = context.worldCollisionNodes.Count( );
|
|
m_nWorldCollisionParamCount = context.worldCollisionParams.Count( );
|
|
m_nFitWeightCount = context.fitWeights.Count();
|
|
m_nFitMatrixCount[ 0 ] = context.fitMatrices.Count();
|
|
m_nFitMatrixCount[ 1 ] = context.m_nFitMatrices1;
|
|
m_nFitMatrixCount[ 2 ] = context.m_nFitMatrices2;
|
|
m_nReverseOffsetCount = m_ReverseOffsets.Count();
|
|
|
|
m_nTaperedCapsuleStretchCount = m_TaperedCapsuleStretches.Count();
|
|
m_nTaperedCapsuleRigidCount = m_TaperedCapsuleRigids.Count();
|
|
m_nSphereRigidCount = m_SphereRigids.Count();
|
|
m_nFreeNodeCount = m_FreeNodes.Count();
|
|
context.nNodeIntegratorCount = GetDampingFlags( ) ? m_nNodeCount : 0;
|
|
|
|
context.nStringsMemSize = 0;
|
|
context.nCtrlNameCount = 0; // if there are no names given, there's no reason to create the array of ctrl names (or their hashes)
|
|
for ( int nCtrl = 0; nCtrl < m_CtrlToNode.Count( ); ++nCtrl )
|
|
{
|
|
int nNode = m_CtrlToNode[ nCtrl ];
|
|
if ( nNode >= 0 )
|
|
{
|
|
const char *pName = m_Nodes[ nNode ].pName;
|
|
if ( pName && *pName )
|
|
{
|
|
// there's a non-empty name
|
|
context.nStringsMemSize += V_strlen( pName ) + 1;
|
|
context.nCtrlNameCount = m_nCtrlCount;
|
|
}
|
|
}
|
|
}
|
|
|
|
if ( false ) // sorting is not strictly necessary and it obscures debugging
|
|
{
|
|
HeapSort( m_TaperedCapsuleStretches, []( const FeTaperedCapsuleStretch_t &left, const FeTaperedCapsuleStretch_t &right ) {
|
|
if ( left.nNode[ 1 ] != right.nNode[ 1 ] )
|
|
return left.nNode[ 1 ] < right.nNode[ 1 ];
|
|
if ( left.nNode[ 0 ] != right.nNode[ 0 ] )
|
|
return left.nNode[ 0 ] < right.nNode[ 0 ];
|
|
if ( left.flRadius[ 1 ] != right.flRadius[ 1 ] )
|
|
return left.flRadius[ 1 ] > right.flRadius[ 1 ];
|
|
if ( left.flRadius[ 0 ] != right.flRadius[ 0 ] )
|
|
return left.flRadius[ 0 ] > right.flRadius[ 0 ];
|
|
return false;
|
|
} );
|
|
RemoveDuplicates( m_TaperedCapsuleStretches );
|
|
HeapSort( m_TaperedCapsuleRigids, []( const FeTaperedCapsuleRigid_t &left, const FeTaperedCapsuleRigid_t &right ) {
|
|
return left.nNode < right.nNode;
|
|
} );
|
|
HeapSort( m_SphereRigids, []( const FeSphereRigid_t & left, const FeSphereRigid_t &right ) {
|
|
return left.nNode < right.nNode;
|
|
} );
|
|
}
|
|
ReallocateMultiBuffer( context );
|
|
|
|
if ( !m_bUnitlessDamping )
|
|
{
|
|
// make a pass over integrators - the dynamic node damping is defined w.r.t. mass outside of the FE model builder. But inside Fe model simulator, nodes are massless (only mass ratios are used)
|
|
// so we need to premultiply some entities by invMass
|
|
for ( uint i = m_nStaticNodes; i < context.nNodeIntegratorCount; ++i )
|
|
{
|
|
m_pNodeIntegrator[ i ].flPointDamping *= m_Nodes[ i ].invMass;
|
|
}
|
|
}
|
|
m_nTreeDepth = ComputeCollisionTreeDepthTopDown();
|
|
|
|
#ifdef DBGFLAG_ASSERTDEBUG
|
|
if ( m_nQuadCount[ 0 ] == m_Elems.Count() )
|
|
{
|
|
for ( uint nCtrl = 0; nCtrl < m_nQuadCount[ 2 ]; ++nCtrl )
|
|
{
|
|
Assert( m_Elems[ nCtrl ].nStaticNodes == 2 );
|
|
}
|
|
|
|
for ( uint nCtrl = m_nQuadCount[ 2 ]; nCtrl < m_nQuadCount[ 1 ]; ++nCtrl )
|
|
{
|
|
Assert( m_Elems[ nCtrl ].nStaticNodes == 1 );
|
|
}
|
|
for ( uint nCtrl = m_nQuadCount[ 1 ]; nCtrl < m_nQuadCount[ 0 ]; ++nCtrl )
|
|
{
|
|
Assert( m_Elems[ nCtrl ].nStaticNodes == 0 );
|
|
}
|
|
}
|
|
|
|
for ( uint nQuad = 0; nQuad < m_nQuadCount[ 2 ]; ++nQuad )
|
|
{
|
|
Assert( m_pQuads[ nQuad ].nNode[ 0 ] < m_nStaticNodes && m_pQuads[ nQuad ].nNode[ 1 ] < m_nStaticNodes && m_pQuads[ nQuad ].nNode[ 2 ] >= m_nStaticNodes && m_pQuads[ nQuad ].nNode[ 3 ] >= m_nStaticNodes );
|
|
}
|
|
|
|
for ( uint nQuad = m_nQuadCount[ 2 ]; nQuad < m_nQuadCount[ 1 ]; ++nQuad )
|
|
{
|
|
Assert( m_pQuads[ nQuad ].nNode[ 0 ] < m_nStaticNodes && m_pQuads[ nQuad ].nNode[ 1 ] >= m_nStaticNodes && m_pQuads[ nQuad ].nNode[ 2 ] >= m_nStaticNodes && m_pQuads[ nQuad ].nNode[ 3 ] >= m_nStaticNodes );
|
|
}
|
|
for ( uint nQuad = m_nQuadCount[ 1 ]; nQuad < m_nQuadCount[ 0 ]; ++nQuad )
|
|
{
|
|
Assert( m_pQuads[ nQuad ].nNode[ 0 ] >= m_nStaticNodes && m_pQuads[ nQuad ].nNode[ 1 ] >= m_nStaticNodes && m_pQuads[ nQuad ].nNode[ 2 ] >= m_nStaticNodes && m_pQuads[ nQuad ].nNode[ 3 ] >= m_nStaticNodes );
|
|
}
|
|
|
|
|
|
Assert( m_nQuadCount[ 0 ] >= m_nQuadCount[ 1 ] && m_nQuadCount[ 1 ] >= m_nQuadCount[ 2 ] );
|
|
|
|
for ( uint nRod = 0; nRod < m_nRodCount; ++nRod )
|
|
{
|
|
AssertDbg( Is0To1( m_pRods[ nRod ].flWeight0 ) && Is0To1( m_pRods[ nRod ].flRelaxationFactor ) );
|
|
}
|
|
#endif
|
|
|
|
return true;
|
|
}
|
|
|
|
void CFeModelBuilder::BuildTree()
|
|
{
|
|
uint nDynNodeCount = GetDynamicNodeCount();
|
|
if ( nDynNodeCount < 2 )
|
|
return;
|
|
uint nStaticBaseIndex = m_nStaticNodes;
|
|
|
|
CFeAgglomerator agg( nDynNodeCount );
|
|
for ( uint nDynNode = 0; nDynNode < nDynNodeCount; ++nDynNode )
|
|
{
|
|
agg.SetNode( nDynNode, m_Nodes[ nDynNode + nStaticBaseIndex ].transform.m_vPosition );
|
|
}
|
|
for ( int nRod = 0; nRod < m_Rods.Count(); ++nRod )
|
|
{
|
|
const FeRodConstraint_t& rod = m_Rods[ nRod ];
|
|
if ( rod.nNode[ 0 ] >= nStaticBaseIndex && rod.nNode[ 1 ] >= nStaticBaseIndex )
|
|
{
|
|
agg.LinkNodes( rod.nNode[ 0 ] - nStaticBaseIndex, rod.nNode[ 1 ] - nStaticBaseIndex );
|
|
}
|
|
}
|
|
for ( int nElemIdx = 0; nElemIdx < m_Elems.Count(); ++nElemIdx )
|
|
{
|
|
const BuildElem_t& elem = m_Elems[ nElemIdx ];
|
|
if ( elem.nStaticNodes < 3 )
|
|
{
|
|
for ( int nElemIndex = elem.nStaticNodes + 1; nElemIndex < ARRAYSIZE( elem.nNode ); ++nElemIndex )
|
|
{
|
|
agg.LinkNodes( elem.nNode[ nElemIndex ] - nStaticBaseIndex, elem.nNode[ nElemIndex - 1 ] - nStaticBaseIndex );
|
|
}
|
|
if ( elem.nStaticNodes == 0 )
|
|
{
|
|
agg.LinkNodes( elem.nNode[ 0 ] - nStaticBaseIndex, elem.nNode[ ARRAYSIZE( elem.nNode ) - 1 ] - nStaticBaseIndex );
|
|
}
|
|
}
|
|
}
|
|
|
|
agg.Build( true );
|
|
|
|
Assert( agg.GetClusterCount() == int( nDynNodeCount * 2 - 1 ) );
|
|
|
|
m_TreeParents.SetCount( agg.GetClusterCount() );
|
|
for ( int nDynNode = 0; nDynNode < agg.GetClusterCount(); ++nDynNode )
|
|
{
|
|
m_TreeParents[ nDynNode ] = agg.GetCluster( nDynNode )->m_nParent;
|
|
}
|
|
|
|
m_TreeChildren.SetCount( agg.GetClusterCount() - nDynNodeCount );
|
|
for ( int nDynNode = nDynNodeCount; nDynNode < agg.GetClusterCount(); ++nDynNode )
|
|
{
|
|
for ( int nChildIndex = 0; nChildIndex < 2; ++nChildIndex )
|
|
{
|
|
uint nChild = agg.GetCluster( nDynNode )->m_nChild[ nChildIndex ];
|
|
m_TreeChildren[ nDynNode - nDynNodeCount ].nChild[ nChildIndex ] = nChild;
|
|
Assert( agg.GetCluster( nChild )->m_nParent == nDynNode );
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
int CFeModelBuilder::FindBuildNodeIndex( const char *pName )
|
|
{
|
|
for ( int i = 0; i < m_Nodes.Count(); ++i )
|
|
{
|
|
if ( !V_stricmp( m_Nodes[ i ].pName, pName ) )
|
|
{
|
|
return i;
|
|
}
|
|
}
|
|
return -1;
|
|
} |