SICALLBACK MOM_SetAttributes_Evaluate( ICENodeContext& in_ctxt )
{
// The current output port being evaluated...
ULONG out_portID = in_ctxt.GetEvaluatedOutputPortID( );
if(gSimulation == NULL)
return CStatus::OK;
switch( out_portID )
{
case ID_OUT_base:
{
CDataArrayLong baseData( in_ctxt, ID_IN_base );
CDataArrayLong idData( in_ctxt, ID_IN_id );
rbdID rbd_ID;
CIndexSet indexSet( in_ctxt );
// Get the output port array ...
CDataArrayLong outData( in_ctxt );
// get all of the input SET data!
CDataArrayBool setPosData( in_ctxt, ID_IN_set_position);
CDataArrayBool setRotData( in_ctxt, ID_IN_set_orientation);
CDataArrayBool setLinvelData( in_ctxt, ID_IN_set_linvelocity);
CDataArrayBool setAngvelData( in_ctxt, ID_IN_set_angvelocity);
CDataArrayBool setStateData( in_ctxt, ID_IN_set_state);
CDataArrayBool setMassData( in_ctxt, ID_IN_set_mass);
CDataArrayBool setBounceData( in_ctxt, ID_IN_set_bounce);
CDataArrayBool setFrictionData( in_ctxt, ID_IN_set_friction);
CDataArrayBool setLindampData( in_ctxt, ID_IN_set_lindamping);
CDataArrayBool setAngdampData( in_ctxt, ID_IN_set_angdamping);
CDataArrayBool setLintreshData( in_ctxt, ID_IN_set_lintreshold);
CDataArrayBool setAngtreshData( in_ctxt, ID_IN_set_angtreshold);
// get all of the input data!
CDataArrayVector3f posData( in_ctxt, ID_IN_position);
CDataArrayVector3f rotData( in_ctxt, ID_IN_orientation);
CDataArrayVector3f linvelData( in_ctxt, ID_IN_linvelocity);
CDataArrayVector3f angvelData( in_ctxt, ID_IN_angvelocity);
CDataArrayLong stateData( in_ctxt, ID_IN_state);
CDataArrayFloat massData( in_ctxt, ID_IN_mass);
CDataArrayFloat bounceData( in_ctxt, ID_IN_bounce);
CDataArrayFloat frictionData( in_ctxt, ID_IN_friction);
CDataArrayFloat lindampData( in_ctxt, ID_IN_lindamping);
CDataArrayFloat angdampData( in_ctxt, ID_IN_angdamping);
CDataArrayFloat lintreshData( in_ctxt, ID_IN_lintreshold);
CDataArrayFloat angtreshData( in_ctxt, ID_IN_angtreshold);
// get the index set iterator
btTransform bodyTransform;
CVector3f bodyPos,linvel,angvel;
btQuaternion bodyRot;
CRotation rot;
CQuaternion quat;
CVector3f anglesf;
CVector3 angles;
for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next())
{
rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]);
rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]);
btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID);
if(bodyRef != NULL)
{
// take care of the positions
if((setPosData.IsConstant() ? setPosData[0] : setPosData[it]) == true)
{
bodyPos = posData.IsConstant() ? posData[0] : posData[it];
bodyTransform = bodyRef->GetWorldTransform();
bodyTransform.setOrigin(btVector3(bodyPos.GetX(),bodyPos.GetY(),bodyPos.GetZ()));
bodyRef->SetWorldTransform(bodyTransform);
}
// take care of the orientations
if((setRotData.IsConstant() ? setRotData[0] : setRotData[it]) == true)
{
anglesf = rotData.IsConstant() ? rotData[0] : rotData[it];
rot.SetFromXYZAngles(DegreesToRadians(anglesf.GetX()),DegreesToRadians(anglesf.GetY()),DegreesToRadians(anglesf.GetZ()));
quat = rot.GetQuaternion();
bodyTransform = bodyRef->GetWorldTransform();
bodyTransform.setRotation(btQuaternion(quat.GetX(),quat.GetY(),quat.GetZ(),quat.GetW()));
bodyRef->SetWorldTransform(bodyTransform);
}
// take care of the linear velocity
if((setLinvelData.IsConstant() ? setLinvelData[0] : setLinvelData[it]) == true)
{
linvel = linvelData.IsConstant() ? linvelData[0] : linvelData[it];
bodyRef->body->setLinearVelocity(btVector3(linvel.GetX(),linvel.GetY(),linvel.GetZ()));
}
// take care of the angular velocity
if((setAngvelData.IsConstant() ? setAngvelData[0] : setAngvelData[it]) == true)
{
angvel = angvelData.IsConstant() ? angvelData[0] : angvelData[it];
bodyRef->body->setAngularVelocity(btVector3(angvel.GetX(),angvel.GetY(),angvel.GetZ()));
}
// take care of the state
if((setStateData.IsConstant() ? setStateData[0] : setStateData[it]) == true)
{
int state = stateData.IsConstant() ? stateData[0] : stateData[it];
if(state == 0)
bodyRef->body->forceActivationState(ACTIVE_TAG);
else if(state == 1)
bodyRef->body->forceActivationState(ISLAND_SLEEPING);
else if(state == 2)
bodyRef->body->forceActivationState(DISABLE_SIMULATION);
}
// take care of the mass
if((setMassData.IsConstant() ? setMassData[0] : setMassData[it]) == true)
{
// compute the inertia
bodyRef->mass = massData.IsConstant() ? massData[0] : massData[it];
btVector3 inertia(0,0,0);
if(bodyRef->mass > 0.0f)
bodyRef->body->getCollisionShape()->calculateLocalInertia(bodyRef->mass,inertia);
bodyRef->body->setMassProps(bodyRef->mass,inertia);
}
// take care of the bounce
if((setBounceData.IsConstant() ? setBounceData[0] : setBounceData[it]) == true)
{
bodyRef->body->setRestitution(bounceData.IsConstant() ? bounceData[0] : bounceData[it]);
}
// take care of the friction
if((setFrictionData.IsConstant() ? setFrictionData[0] : setFrictionData[it]) == true)
{
bodyRef->body->setFriction(frictionData.IsConstant() ? frictionData[0] : frictionData[it]);
}
// take care of the linear damping
if((setLindampData.IsConstant() ? setLindampData[0] : setLindampData[it]) == true)
{
float angdamp = bodyRef->body->getAngularDamping();
bodyRef->body->setDamping(lindampData.IsConstant() ? lindampData[0] : lindampData[it],angdamp);
}
// take care of the angular damping
if((setAngdampData.IsConstant() ? setAngdampData[0] : setAngdampData[it]) == true)
{
float lindamp = bodyRef->body->getLinearDamping();
bodyRef->body->setDamping(lindamp,angdampData.IsConstant() ? angdampData[0] : angdampData[it]);
}
// take care of the linear treshold
if((setLintreshData.IsConstant() ? setLintreshData[0] : setLintreshData[it]) == true)
{
float angtresh = bodyRef->body->getAngularSleepingThreshold();
bodyRef->body->setSleepingThresholds(lintreshData.IsConstant() ? lintreshData[0] : lintreshData[it],angtresh);
}
// take care of the angular treshold
if((setAngtreshData.IsConstant() ? setAngtreshData[0] : setAngtreshData[it]) == true)
{
float lintresh = bodyRef->body->getLinearSleepingThreshold();
bodyRef->body->setSleepingThresholds(lintresh,angtreshData.IsConstant() ? angtreshData[0] : angtreshData[it]);
}
}
outData[it] = rbd_ID.primary;
}
break;
}
};
return CStatus::OK;
}
/** "Intersecting a Ray with a Cylinder" by Joseph M. Cychosz and Warren N. Waggenspack, Jr., ([email protected], [email protected]) in "Graphics Gems IV", Academic Press, 1994 */ Float32 CCylinder::Intersect( const CRay& r ) { Float32 in, out; Bool hit; // True if ray intersects cyl CVector3f rc; // Ray base to cylinder base Float32 dist; // Shortest distance between Float32 t, s; // Distances along the ray CVector3f n, o; Float32 ln; CVector3f axis = GetAxis(); rc = r.Origin - PointA; n.CrossProduct( r.Direction, axis ); ln = n.GetLength(); if ( ln == 0.0f ) { CVector3f tmp; // ray parallel to cyl dist = rc.DotProduct( axis ); tmp = rc - axis * dist; dist = tmp.GetLength(); in = -FLOAT32_MAX_VAL; out = FLOAT32_MAX_VAL; if( dist <= Radius ) // true if ray is in cyl { return 1.0; } } n.Normalize(); dist = MathAbs( rc.DotProduct( n ) ); // shortest distance hit = (dist <= Radius); if (hit) { // if ray hits cylinder o.CrossProduct( rc, axis ); t = - o.DotProduct( n ) / ln; o.CrossProduct( n, axis ); o.Normalize(); s = MathAbs( MathSqrt( Radius*Radius - dist*dist) / r.Direction.DotProduct( o ) ); in = t - s; // entering distance out = t + s; // exiting distance // Test for capped cylinder CVector3f intersection = r.Origin + r.Direction * in; CVector3f cylBaseToInter = intersection - PointA; ln = cylBaseToInter.GetLength(); cylBaseToInter.Normalize(); if( axis.DotProduct( cylBaseToInter ) >= 0.0f ) { Float32 cylLength = CVector3f( PointA-PointB ).GetLength(); if( ln <= cylLength ) return in; } } return -1.0f; }
// intersect a box
bool TXDualEdge::Touch(const CVector3f& minp, const CVector3f& maxp) const {
int m = 4;
CVector3f step;
step.Sub(m_dp[1], m_dp[0]);
step.ScaleInPlace(1/4);
CVector3f A = m_dp[0];
CVector3f B;
B.Add(m_dp[0],step);
for (int i = 0; i < m; i++) {
CVector3f bmin(MIN(A.GetX(),B.GetX()),MIN(A.GetY(),B.GetY()),MIN(A.GetZ(),B.GetZ()));
CVector3f bmax(MAX(A.GetX(),B.GetX()),MAX(A.GetY(),B.GetY()),MAX(A.GetZ(),B.GetZ()));
if (bmin.GetX() <= maxp.GetX() && minp.GetX() <= bmax.GetX() &&
bmin.GetY() <= maxp.GetY() && minp.GetY() <= bmax.GetY() &&
bmin.GetZ() <= maxp.GetZ() && minp.GetZ() <= bmax.GetZ()) return true;
A = B;
B += step;
}
return false;
}
void readBoundingBoxBig(athena::io::IStreamReader& in) {
min.readBig(in);
max.readBig(in);
}
CVector3f CCylinder::GetAxis() const
{
CVector3f tmp = PointB - PointA;
tmp.Normalize();
return tmp;
}
Void CAABox::AddPoint( const CVector3f& point )
{
if( IsEmpty() )
{
m_Minimum = point;
m_Maximum = point;
}
else
{
if ( point.GetX() > m_Maximum.GetX() )
m_Maximum.SetX( point.GetX() );
if ( point.GetY() > m_Maximum.GetY() )
m_Maximum.SetY( point.GetY() );
if ( point.GetZ() > m_Maximum.GetZ() )
m_Maximum.SetZ( point.GetZ() );
if ( point.GetX() < m_Minimum.GetX() )
m_Minimum.SetX( point.GetX() );
if ( point.GetY() < m_Minimum.GetY() )
m_Minimum.SetY( point.GetY() );
if ( point.GetZ() < m_Minimum.GetZ() )
m_Minimum.SetZ( point.GetZ() );
}
}
bool invalid() { return (max.x() < min.x() || max.y() < min.y() || max.z() < min.z()); }
CVector3f furthestPointAlongVector(const CVector3f& other) const {
return {(other.x() >= 0.f ? max.x() : min.x()), (other.y() >= 0.f ? max.y() : min.y()),
(other.z() >= 0.f ? max.z() : min.z())};
}
float distanceBetween(const CAABox& other) {
int intersects = 0;
if (max.x() >= other.min.x() && min.x() <= other.max.x())
intersects |= 0x1;
if (max.y() >= other.min.y() && min.y() <= other.max.y())
intersects |= 0x2;
if (max.z() >= other.min.z() && min.z() <= other.max.z())
intersects |= 0x4;
float minX, maxX;
if (max.x() < other.min.x()) {
minX = max.x();
maxX = other.min.x();
} else {
minX = min.x();
maxX = other.max.x();
}
float minY, maxY;
if (max.y() < other.min.y()) {
minY = max.y();
maxY = other.min.y();
} else {
minY = min.y();
maxY = other.max.y();
}
float minZ, maxZ;
if (max.z() < other.min.z()) {
minZ = max.z();
maxZ = other.min.z();
} else {
minZ = min.z();
maxZ = other.max.z();
}
switch (intersects) {
case 0:
return zeus::CVector3f(maxX - minX, maxY - minY, maxZ - minZ).magnitude();
case 1:
return zeus::CVector2f(maxY - minY, maxZ - minZ).magnitude();
case 2:
return zeus::CVector2f(maxX - minX, maxZ - minZ).magnitude();
case 3:
return std::fabs(maxZ - minZ);
case 4:
return zeus::CVector2f(maxX - minX, maxY - minY).magnitude();
case 5:
return std::fabs(maxY - minY);
case 6:
return std::fabs(maxX - minX);
case 7:
default:
return 0.f;
}
}
void accumulateBounds(const CVector3f& point) {
if (min.x() > point.x())
min.x() = point.x();
if (min.y() > point.y())
min.y() = point.y();
if (min.z() > point.z())
min.z() = point.z();
if (max.x() < point.x())
max.x() = point.x();
if (max.y() < point.y())
max.y() = point.y();
if (max.z() < point.z())
max.z() = point.z();
}
bool pointInside(const CVector3f& other) const {
return (min.x() <= other.x() && other.x() <= max.x() && min.y() <= other.y() && other.y() <= max.y() &&
min.z() <= other.z() && other.z() <= max.z());
}
CLineSeg getEdge(EBoxEdgeId id) const {
switch (id) {
case EBoxEdgeId::Z0:
default:
return CLineSeg({min.x(), min.y(), max.z()}, {min.x(), min.y(), min.z()});
case EBoxEdgeId::X0:
return CLineSeg({min.x(), min.y(), min.z()}, {max.x(), min.y(), min.z()});
case EBoxEdgeId::Z1:
return CLineSeg({max.x(), min.y(), min.z()}, {max.x(), min.y(), max.z()});
case EBoxEdgeId::X1:
return CLineSeg({max.x(), min.y(), max.z()}, {min.x(), min.y(), max.z()});
case EBoxEdgeId::Z2:
return CLineSeg({max.x(), max.y(), max.z()}, {max.x(), max.y(), min.z()});
case EBoxEdgeId::X2:
return CLineSeg({max.x(), max.y(), min.z()}, {min.x(), max.y(), min.z()});
case EBoxEdgeId::Z3:
return CLineSeg({min.x(), max.y(), min.z()}, {min.x(), max.y(), max.z()});
case EBoxEdgeId::X3:
return CLineSeg({min.x(), max.y(), max.z()}, {max.x(), max.y(), max.z()});
case EBoxEdgeId::Y0:
return CLineSeg({min.x(), min.y(), max.z()}, {min.x(), max.y(), max.z()});
case EBoxEdgeId::Y1:
return CLineSeg({min.x(), min.y(), min.z()}, {min.x(), max.y(), min.z()});
case EBoxEdgeId::Y2:
return CLineSeg({max.x(), min.y(), min.z()}, {max.x(), max.y(), min.z()});
case EBoxEdgeId::Y3:
return CLineSeg({max.x(), min.y(), max.z()}, {max.x(), max.y(), max.z()});
}
}
int CEditModel::CalAniSize(const CEditSkeletal& Skeletal, CEditAnimate& Animate, float Size)
{
int i,j,pos=0;
CVector3f child[256];
float len=0;
BYTE SID = Skeletal.GetSkeletalID();
if( SID && Skeletal.GetChildNum() )
{
for( i = 0; i < (int)Skeletal.GetChildNum(); i++ )
{
BYTE ID = Skeletal.GetChild(i)->GetSkeletalID();
CQuaternion Quat;
CVector3f Tran;
Animate.GetKeyFrame( ID )->GetFirst(Quat,Tran,0.0f);
CVector3f off = Tran;
len = max( off.Mag(), len );
for( j = 0; j < pos; j++ )
{
if( child[j] == off )
break;
}
if( j == pos )
{
child[pos] = off;
pos++;
}
}
if( pos == 1 )//代表只有一个子骨骼
{
m_SkeletalPos[SID].m_Scale = sqr::CVector3f(Size*0.1f+child[0].y,child[0].x,Size*0.1f+child[0].z);
}
else
{
float p1[3]={ 0, 0, 0 };
float p2[3]={ 0, 0, 0 };
for( i=0; i<pos; i++ )
{
if( child[i].x>p2[0] )
p2[0] = child[i].x;
if( child[i].y>p2[1] )
p2[1] = child[i].y;
if( child[i].z>p2[2] )
p2[2] = child[i].z;
if( child[i].x<p1[0] )
p1[0] = child[i].x;
if( child[i].y<p1[1] )
p1[1] = child[i].y;
if( child[i].z<p1[2] )
p1[2] = child[i].z;
}
float height = abs(p1[2]-p2[2]);
float width = abs(p1[0]-p2[0]);
float length = abs(p1[1]-p2[1]);
m_SkeletalPos[SID].m_Scale = sqr::CVector3f(width,height,length);
}
}
else
{
float s = Size*0.1f;
m_SkeletalPos[SID].m_Scale = sqr::CVector3f(s,4.0f,s);
}
//for( i = 0; i<6; i++ )
//{
// m_SkeletalPos[SID].m_SklPos[i].np = m_SkeletalPos[SID].m_SklPos[i].p.Normalize();
//}
for( i = 0; i < (int)Skeletal.GetChildNum(); i++ )
CalAniSize( *(CEditSkeletal*)Skeletal.GetChild(i), Animate, Size );
return 0;
}
bool COBBox::OBBIntersectsBox(const COBBox& other) const {
CVector3f v = other.transform.origin - transform.origin;
CVector3f T = CVector3f(v.dot(transform.basis[0]), v.dot(transform.basis[1]), v.dot(transform.basis[2]));
CMatrix3f R;
float ra, rb, t;
for (int i = 0; i < 3; ++i)
for (int k = 0; k < 3; ++k)
R[i][k] = transform.basis[i].dot(other.transform.basis[k]);
for (int i = 0; i < 3; ++i) {
ra = extents[i];
rb = (other.extents[0] * std::fabs(R[i][0])) + (other.extents[1] * std::fabs(R[i][1])) +
(other.extents[2] * std::fabs(R[i][2]));
t = std::fabs(T[i]);
if (t > (ra + rb + FLT_EPSILON))
return false;
}
for (int k = 0; k < 3; ++k) {
ra = (extents[0] * std::fabs(R[0][k])) + (extents[1] * std::fabs(R[1][k])) + (extents[2] * std::fabs(R[2][k]));
rb = other.extents[k];
t = std::fabs(T[0] * R[0][k] + T[1] * R[1][k] + T[2] * R[2][k]);
if (t > (ra + rb + FLT_EPSILON))
return false;
}
/* A0 x B0 */
ra = (extents[1] * std::fabs(R[2][0])) + (extents[2] * std::fabs(R[1][0]));
rb = (other.extents[1] * std::fabs(R[0][2])) + (other.extents[2] * std::fabs(R[0][1]));
t = std::fabs((T[2] * R[1][0]) - (T[1] * R[2][0]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A0 x B1 */
ra = (extents[1] * std::fabs(R[2][1])) + (extents[2] * std::fabs(R[1][1]));
rb = (other.extents[0] * std::fabs(R[0][2])) + (other.extents[2] * std::fabs(R[0][0]));
t = std::fabs((T[2] * R[1][1]) - (T[1] * R[2][1]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A0 x B2 */
ra = (extents[1] * std::fabs(R[2][2])) + (extents[2] * std::fabs(R[1][2]));
rb = (other.extents[0] * std::fabs(R[0][1])) + (other.extents[1] * std::fabs(R[0][0]));
t = std::fabs((T[2] * R[1][2]) - (T[1] * R[2][2]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A1 x B0 */
ra = (extents[0] * std::fabs(R[2][0])) + (extents[2] * std::fabs(R[0][0]));
rb = (other.extents[1] * std::fabs(R[1][2])) + (other.extents[2] * std::fabs(R[1][1]));
t = std::fabs((T[0] * R[2][0]) - (T[2] * R[0][0]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A1 x B1 */
ra = (extents[0] * std::fabs(R[2][1])) + (extents[2] * std::fabs(R[0][1]));
rb = (other.extents[0] * std::fabs(R[1][2])) + (other.extents[2] * std::fabs(R[1][0]));
t = std::fabs((T[0] * R[2][1]) - (T[2] * R[0][1]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A1 x B2 */
ra = (extents[0] * std::fabs(R[2][2])) + (extents[2] * std::fabs(R[0][2]));
rb = (other.extents[0] * std::fabs(R[1][1])) + (other.extents[1] * std::fabs(R[1][0]));
t = std::fabs((T[0] * R[2][2]) - (T[2] * R[0][2]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A2 x B0 */
ra = (extents[0] * std::fabs(R[1][0])) + (extents[1] * std::fabs(R[0][0]));
rb = (other.extents[1] * std::fabs(R[2][2])) + (other.extents[2] * std::fabs(R[2][1]));
t = std::fabs((T[1] * R[0][0]) - (T[0] * R[1][0]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A2 x B1 */
ra = (extents[0] * std::fabs(R[1][1])) + (extents[1] * std::fabs(R[0][1]));
rb = (other.extents[0] * std::fabs(R[2][2])) + (other.extents[2] * std::fabs(R[2][0]));
t = std::fabs((T[1] * R[0][1]) - (T[0] * R[1][1]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A2 x B2 */
ra = (extents[0] * std::fabs(R[1][2])) + (extents[1] * std::fabs(R[0][2]));
rb = (other.extents[0] * std::fabs(R[2][1])) + (other.extents[1] * std::fabs(R[2][0]));
t = std::fabs((T[1] * R[0][2]) - (T[0] * R[1][2]));
if (t > (ra + rb + FLT_EPSILON))
return false;
return true;
}
Void CGameProperty::SetAsVector3( const CVector3f& d)
{
m_Tuple3fValue.x = d.GetX();
m_Tuple3fValue.y = d.GetY();
m_Tuple3fValue.z = d.GetZ();
}
CVector3f clampToBox(const CVector3f& vec) const {
CVector3f ret = vec;
ret.x() = clamp(min.x(), float(ret.x()), max.x());
ret.y() = clamp(min.y(), float(ret.y()), max.y());
ret.z() = clamp(min.z(), float(ret.z()), max.z());
return ret;
}
// intersect a ray with the mesh
bool TXGrid3D::IntersectRay(const CVector3f& start, const CVector3f& end)
{
// pick rays
m_rays.push_back(TXRay(start, end));
CVector3f dir;
dir.Sub(start,end);
int idir[3] = {
fabs(dir.GetX()) < DBL_EPSILON ? 0 : (dir.GetX() > 0 ? 1 : -1),
fabs(dir.GetY()) < DBL_EPSILON ? 0 : (dir.GetY() > 0 ? 1 : -1),
fabs(dir.GetZ()) < DBL_EPSILON ? 0 : (dir.GetZ() > 0 ? 1 : -1) };
double dist = 0;
CVector3f pos;
pos.Sub(end, m_min);
int ix = (int)floor((end.GetX() - m_min.GetX())/m_xstep),
iy = (int)floor((end.GetY() - m_min.GetY())/m_ystep),
iz = (int)floor((end.GetZ() - m_min.GetZ())/m_zstep);
double tx = 1.0, ty = 1.0, tz = 1.0;
// first cell
ix = MIN(MAX(ix, 0), m_size-1);
iy = MIN(MAX(iy, 0), m_size-1);
iz = MIN(MAX(iz, 0), m_size-1);
// intersection test, from end to start
while ((dist < 1) && (ix >= 0) && (ix < m_size) &&(iy >= 0) && (iy < m_size) && (iz >= 0) && (iz < m_size))
{
int csz = m_grid[ix][iy][iz].size();
for (int i=0; i<csz; i++)
{
TXGridTriangle* gt = m_grid[ix][iy][iz][i];
double rpdot = dir.Dot(gt->m_n);
if (rpdot != 0)
{
CVector3f tmp;
tmp.Sub(gt->m_t->m_v[0]->m_pos,end);
double t = tmp.Dot(gt->m_n) / rpdot;
if (t > DBL_EPSILON && t < 1)
{
CVector3f pt;
pt.ScaleAdd(t,dir,end);
CVector3f pt0,pt1,pt2;
pt0.Sub(pt,gt->m_t->m_v[0]->m_pos);
pt1.Sub(pt,gt->m_t->m_v[1]->m_pos);
pt2.Sub(pt,gt->m_t->m_v[2]->m_pos);
if (pt0.Dot(gt->m_en1)> -DBL_EPSILON &&
pt1.Dot(gt->m_en2)>-DBL_EPSILON &&
pt2.Dot(gt->m_en3)>-DBL_EPSILON)
{
m_rays[m_rays.size()-1].m_p = t;
return true;
}
}
}
}
// next cell
if (idir[0] != 0)
tx = (m_min.GetX() + (ix+(idir[0]+1)/2)*m_xstep - end.GetX()) / dir.GetX();
if (idir[1] != 0)
ty = (m_min.GetY() + (iy+(idir[1]+1)/2)*m_ystep - end.GetY()) / dir.GetY();
if (idir[2] != 0)
tz = (m_min.GetZ() + (iz+(idir[2]+1)/2)*m_zstep - end.GetZ()) / dir.GetZ();
if ((tx <= ty) && (tx <= tz))
{
dist = tx, ix += idir[0];
if (ty == tx) iy += idir[1];
if (tz == tx) iz += idir[2];
}
else if (ty <= tz)
{
dist = ty, iy += idir[1];
if (tz == ty) iz += idir[2];
}
else dist = tz, iz += idir[2];
}
return false;
}
XSIPLUGINCALLBACK CStatus nest_LatticeDeform_Evaluate( ICENodeContext& in_ctxt )
{
// The current output port being evaluated...
ULONG out_portID = in_ctxt.GetEvaluatedOutputPortID( );
switch( out_portID )
{
case Array_ID_OUT_Result:
{
siICENodeDataType dataType;
siICENodeStructureType struType;
siICENodeContextType contType;
in_ctxt.GetPortInfo(Lattice_ID_IN_Point,dataType,struType,contType);
// get all of the data that is the same for any structure
CDataArrayVector3f SubdivData( in_ctxt, Lattice_ID_IN_Subdivision );
CDataArrayVector3f StepData( in_ctxt, Lattice_ID_IN_Step );
CDataArray2DVector3f ReferenceData( in_ctxt, Lattice_ID_IN_Reference );
CDataArray2DVector3f CurrentData( in_ctxt, Lattice_ID_IN_Current );
CDataArray2DVector3f::Accessor ReferenceDataSub = ReferenceData[0];
CDataArray2DVector3f::Accessor CurrentDataSub = CurrentData[0];
// define the things we need to calculate
long subdiv[3];
subdiv[0] = long(floor(SubdivData[0].GetX()));
subdiv[1] = long(floor(SubdivData[0].GetY()));
subdiv[2] = long(floor(SubdivData[0].GetZ()));
long subdiv1[3];
subdiv1[0] = subdiv[0]+1;
subdiv1[1] = subdiv[1]+1;
subdiv1[2] = subdiv[2]+1;
float step[3];
step[0] = 1.0f / StepData[0].GetX();
step[1] = 1.0f / StepData[0].GetY();
step[2] = 1.0f / StepData[0].GetZ();
float steplength = StepData[0].GetLength();
long indexX[8];
long indexY[8];
long indexZ[8];
long index[8];
long lastIndex[3];
lastIndex[0] = -1;
lastIndex[1] = -1;
lastIndex[2] = -1;
CVector3f posCp;
CVector3f pos;
CVector3f diff[8];
CVector3f motion[8];
CVector3f motionScl[8];
CVector3f deform;
float weight[8];
float xyz0[3];
float xyz1[3];
float weightSum;
if(struType == siICENodeStructureSingle)
{
// two behaviours based on the datatype...
// Get the output port array ...
CDataArrayVector3f outData( in_ctxt );
// Get the input data buffers for each port
CDataArrayVector3f PointData( in_ctxt, Lattice_ID_IN_Point );
// iterate each subset!
CIndexSet IndexSet( in_ctxt );
for(CIndexSet::Iterator it = IndexSet.Begin(); it.HasNext(); it.Next())
{
// first let's find the index inside the box!
posCp.Set(PointData[it].GetX(),PointData[it].GetY(),PointData[it].GetZ());
// substract the lowest corner
pos.Sub(posCp,ReferenceDataSub[0]);
pos.Set(pos.GetX() * step[0], pos.GetY() * step[1], pos.GetZ() * step[2]);
xyz0[0] = pos.GetX() - floor(pos.GetX());
xyz0[1] = pos.GetY() - floor(pos.GetY());
xyz0[2] = pos.GetZ() - floor(pos.GetZ());
xyz1[0] = 1.0 - xyz0[0];
xyz1[1] = 1.0 - xyz0[1];
xyz1[2] = 1.0 - xyz0[2];
// calculate the indices (decomposed)
indexX[0] = clampl(long(floor(pos.GetX())),0,subdiv[0]);
indexY[0] = clampl(long(floor(pos.GetY())),0,subdiv[1]);
indexZ[0] = clampl(long(floor(pos.GetZ())),0,subdiv[2]);
if(lastIndex[0] != indexX[0] || lastIndex[1] != indexY[0] || lastIndex[2] != indexZ[0])
{
indexX[1] = clampl(indexX[0]+1 ,0,subdiv[0]);
indexY[1] = clampl(indexY[0] ,0,subdiv[1]);
indexZ[1] = clampl(indexZ[0] ,0,subdiv[2]);
indexX[2] = clampl(indexX[0]+1 ,0,subdiv[0]);
indexY[2] = clampl(indexY[0]+1 ,0,subdiv[1]);
indexZ[2] = clampl(indexZ[0] ,0,subdiv[2]);
indexX[3] = clampl(indexX[0]+1 ,0,subdiv[0]);
indexY[3] = clampl(indexY[0] ,0,subdiv[1]);
indexZ[3] = clampl(indexZ[0]+1 ,0,subdiv[2]);
indexX[4] = clampl(indexX[0]+1 ,0,subdiv[0]);
indexY[4] = clampl(indexY[0]+1 ,0,subdiv[1]);
indexZ[4] = clampl(indexZ[0]+1 ,0,subdiv[2]);
indexX[5] = clampl(indexX[0] ,0,subdiv[0]);
indexY[5] = clampl(indexY[0]+1 ,0,subdiv[1]);
indexZ[5] = clampl(indexZ[0] ,0,subdiv[2]);
indexX[6] = clampl(indexX[0] ,0,subdiv[0]);
indexY[6] = clampl(indexY[0] ,0,subdiv[1]);
indexZ[6] = clampl(indexZ[0]+1 ,0,subdiv[2]);
indexX[7] = clampl(indexX[0] ,0,subdiv[0]);
indexY[7] = clampl(indexY[0]+1 ,0,subdiv[1]);
indexZ[7] = clampl(indexZ[0]+1 ,0,subdiv[2]);
for(int i=0;i<8;i++)
{
// compose the indices!
index[i] = compose(indexX[i],indexY[i],indexZ[i],subdiv1[1],subdiv1[2]);
// calculate the motions
motion[i].Sub(CurrentDataSub[index[i]],ReferenceDataSub[index[i]]);
}
}
else
{
// for performance, remember the last used index
lastIndex[0] = indexX[0];
lastIndex[1] = indexY[0];
lastIndex[2] = indexZ[0];
}
// compute the weights
weight[0] = xyz1[0] * xyz1[1] * xyz1[2];
weight[1] = xyz0[0] * xyz1[1] * xyz1[2];
weight[2] = xyz0[0] * xyz0[1] * xyz1[2];
weight[3] = xyz0[0] * xyz1[1] * xyz0[2];
weight[4] = xyz0[0] * xyz0[1] * xyz0[2];
weight[5] = xyz1[0] * xyz0[1] * xyz1[2];
weight[6] = xyz1[0] * xyz1[1] * xyz0[2];
weight[7] = xyz1[0] * xyz0[1] * xyz0[2];
// sum up all weighted motions
deform.SetNull();
for(int i=0;i<8;i++)
{
motionScl[i].Scale(weight[i],motion[i]);
deform.AddInPlace(motionScl[i]);
}
// output the deformed position
outData[it] = deform;
}
}
else
{
// two behaviours based on the datatype...
// Get the output port array ...
CDataArray2DVector3f outData( in_ctxt );
// Get the input data buffers for each port
CDataArray2DVector3f PointData( in_ctxt, Lattice_ID_IN_Point );
// iterate each subset!
CIndexSet IndexSet( in_ctxt );
for(CIndexSet::Iterator it = IndexSet.Begin(); it.HasNext(); it.Next())
{
CDataArray2DVector3f::Accessor PointDataSub = PointData[it];
long subCount = PointDataSub.GetCount();
Application().LogMessage(CString((LONG)subCount));
outData.Resize(it,subCount);
for(long k=0;k<subCount;k++)
{
// first let's find the index inside the box!
posCp.Set(PointDataSub[k].GetX(),PointDataSub[k].GetY(),PointDataSub[k].GetZ());
// substract the lowest corner
pos.Sub(posCp,ReferenceDataSub[0]);
pos.Set(pos.GetX() * step[0], pos.GetY() * step[1], pos.GetZ() * step[2]);
xyz0[0] = pos.GetX() - floor(pos.GetX());
xyz0[1] = pos.GetY() - floor(pos.GetY());
xyz0[2] = pos.GetZ() - floor(pos.GetZ());
xyz1[0] = 1.0 - xyz0[0];
xyz1[1] = 1.0 - xyz0[1];
xyz1[2] = 1.0 - xyz0[2];
// calculate the indices (decomposed)
indexX[0] = clampl(long(floor(pos.GetX())),0,subdiv[0]);
indexY[0] = clampl(long(floor(pos.GetY())),0,subdiv[1]);
indexZ[0] = clampl(long(floor(pos.GetZ())),0,subdiv[2]);
if(lastIndex[0] != indexX[0] || lastIndex[1] != indexY[0] || lastIndex[2] != indexZ[0])
{
indexX[1] = clampl(indexX[0]+1 ,0,subdiv[0]);
indexY[1] = clampl(indexY[0] ,0,subdiv[1]);
indexZ[1] = clampl(indexZ[0] ,0,subdiv[2]);
indexX[2] = clampl(indexX[0]+1 ,0,subdiv[0]);
indexY[2] = clampl(indexY[0]+1 ,0,subdiv[1]);
indexZ[2] = clampl(indexZ[0] ,0,subdiv[2]);
indexX[3] = clampl(indexX[0]+1 ,0,subdiv[0]);
indexY[3] = clampl(indexY[0] ,0,subdiv[1]);
indexZ[3] = clampl(indexZ[0]+1 ,0,subdiv[2]);
indexX[4] = clampl(indexX[0]+1 ,0,subdiv[0]);
indexY[4] = clampl(indexY[0]+1 ,0,subdiv[1]);
indexZ[4] = clampl(indexZ[0]+1 ,0,subdiv[2]);
indexX[5] = clampl(indexX[0] ,0,subdiv[0]);
indexY[5] = clampl(indexY[0]+1 ,0,subdiv[1]);
indexZ[5] = clampl(indexZ[0] ,0,subdiv[2]);
indexX[6] = clampl(indexX[0] ,0,subdiv[0]);
indexY[6] = clampl(indexY[0] ,0,subdiv[1]);
indexZ[6] = clampl(indexZ[0]+1 ,0,subdiv[2]);
indexX[7] = clampl(indexX[0] ,0,subdiv[0]);
indexY[7] = clampl(indexY[0]+1 ,0,subdiv[1]);
indexZ[7] = clampl(indexZ[0]+1 ,0,subdiv[2]);
for(int i=0;i<8;i++)
{
// compose the indices!
index[i] = compose(indexX[i],indexY[i],indexZ[i],subdiv1[1],subdiv1[2]);
// calculate the motions
motion[i].Sub(CurrentDataSub[index[i]],ReferenceDataSub[index[i]]);
}
}
else
{
// for performance, remember the last used index
lastIndex[0] = indexX[0];
lastIndex[1] = indexY[0];
lastIndex[2] = indexZ[0];
}
// compute the weights
weight[0] = xyz1[0] * xyz1[1] * xyz1[2];
weight[1] = xyz0[0] * xyz1[1] * xyz1[2];
weight[2] = xyz0[0] * xyz0[1] * xyz1[2];
weight[3] = xyz0[0] * xyz1[1] * xyz0[2];
weight[4] = xyz0[0] * xyz0[1] * xyz0[2];
weight[5] = xyz1[0] * xyz0[1] * xyz1[2];
weight[6] = xyz1[0] * xyz1[1] * xyz0[2];
weight[7] = xyz1[0] * xyz0[1] * xyz0[2];
// sum up all weighted motions
deform.SetNull();
for(int i=0;i<8;i++)
{
motionScl[i].Scale(weight[i],motion[i]);
deform.AddInPlace(motionScl[i]);
}
// output the deformed position
outData[it][k] = deform;
}
}
}
}
break;
// Other output ports...
};
return CStatus::OK;
}
