#5 Split up Constraint/Force Code

[Daniel Kramer]

I would definitely split processSubData into two functions,
processConstraints and procesForces. processConstraints
should be
something like:

// Initialize all constraints. This gives constraints a
chance to
// prepare themselves for use, such as by looking up their
// goal objects.
SIM_ConstraintIterator::initConstraints(*object, time);

SIM_DataFilterByType anchorFilter("SIM_ConAnchor");
SIM_ConstratinIterator conit(*object, 0, &anchorFilter,
&anchorFilter, time);

for (conit.rewind(); !conit.atEnd(); conit.advance())
{
SIM_ConRel *conrel;
const SIM_ConAnchorSpatial *spanchor,
*goalspanchor;
const SIM_ConAnchorRotational *rotanchor,
*goalrotanchor;

conrel = conit.getRelationship();
spanchor = SIM_DATA_CASTCONST(it.getCurrentAnchor(),

SIM_ConAnchorSpatial);
goalspanchor =
SIM_DATA_CASTCONST(it.getGoalAnchor(),

SIM_ConAnchorSpatial);
if( spanchor && goalspanchor )
{
// Handle spatial constraints.
}

rotanchor =
SIM_DATA_CASTCONST(it.getCurrentAnchor(),

SIM_ConAnchorRotational);
goalrotanchor =
SIM_DATA_CASTCONST(it.getGoalAnchor(),

SIM_ConAnchorRotational);
if( rotanchor && goalrotanchor )
{
// Handle Rotational constraints.
}
}

As for forces, you don't need to handle each force type
individually.
You should use the functions on the SIM_Force base
class. Also, there
are functions for making a calback for all data that
matches a
particular data type. So processForces would look
something like:

///
/// This is a helper class to iterate over all the
forces on an object
/// and sum them up.
///
class ODE_EachForceCallback : public SIM_EachDataCallback
{
public:
ODE_EachForceCallback(const
UT_Vector3 &pos,
const
UT_Vector3 &vel,
const
UT_Vector3 &angvel,
const fpreal
mass,
UT_Vector3
&force,
UT_Vector3
&torque)
: myPosition(pos),
myVelocity(vel),
myAngVel(angvel),
myMass(mass),
myForce(force),
myTorque(torque)
{ }
virtual ~ODE_EachForceCallback()
{ }

virtual void callbackConst(const
SIM_Data *data,
const
char *name);

private:
UT_Vector3 myPosition;
UT_Vector3 myVelocity;
UT_Vector3 myAngVel;
fpreal myMass;
UT_Vector3 &myForce;
UT_Vector3 &myTorque;
};

void
ODE_EachForceCallback::callbackConst(const SIM_Data
*data, const char *)
{
const SIM_Force *forcedata =
SIM_DATA_CASTCONST(data,

SIM_Force);
UT_Vector3 force, torque;

forcedata->getForce(myPosition, myVelocity, myAngVel,
myMass, force, torque);
myForce += force;
myTorque += torque;
}

processForces(SIM_Object *object)
{
// Grab the velocity, angular velocity, mass,
and center of
// mass fromthe object (from the Position data).

UT_Vector3 sumforces(0.0, 0.0, 0.0);
UT_Vector3 sumtorque(0.0, 0.0, 0.0);
ODE_EachForceCallback callback(com,
vel,
angvel,
mass,
sumforces,
sumtorques);

object->getObject()->forEachConstSubData(callback,

SIM_DataFilterByType("SIM_Force"),
SIM_FORCES_DATANAME,
SIM_DataFilterNone());

// Apply sumforces and sumtorques to object...
}

I also noticed you calling getNthConstSubData to grab
the force and
constraint data. You should avoid this function
whenever possible. If
you call this function for each subdata, you've created
an O(n^2)
algorithm on the number of subdata.