void AbstractSortingStrategy::handleItem(AbstractGroupableItem *item)
{
//qDebug() << item->name();
if (item->itemType() == GroupItemType) {
handleGroup(qobject_cast<TaskGroup*>(item));
} else if (item->itemType() == TaskItemType && !(qobject_cast<TaskItem*>(item))->task()) { //ignore startup tasks
connect(item, SIGNAL(gotTaskPointer()), this, SLOT(check())); //sort the task as soon as it is a real one
return;
}
check(item);
}
static u_result
startMonitoring(
const u_participant participant,
const u_waitset waitset,
const struct builtin_datareader_set *drset)
{
c_iter events, topics;
u_waitsetEvent event;
c_time timeout;
os_uint32 reportInterval;
v_gid participantGid, publicationGid, subscriptionGid, gid;
u_result result;
u_dataReader dataReader;
u_topic topic;
c_iter vgroups;
v_group vgroup;
v_duration duration;
c_long participantOffset, publicationOffset, subscriptionOffset;
os_threadAttr attr;
os_result osr;
/*Resolve unique identifications of readers*/
participantGid = u_entityGid((u_entity)drset->participant_dr);
publicationGid = u_entityGid((u_entity)drset->publication_dr);
subscriptionGid = u_entityGid((u_entity)drset->subscription_dr);
/*Resolve topics to find offsets in the data. The offsets are used later on*/
duration.seconds = 0;
duration.nanoseconds = 0;
topics = u_participantFindTopic(participant, V_PARTICIPANTINFO_NAME, duration);
topic = c_iterTakeFirst(topics);
if(topic){
result = u_entityAction(u_entity(topic), resolveOffset, &participantOffset);
} else {
result = U_RESULT_INTERNAL_ERROR;
in_printf(IN_LEVEL_SEVERE, "Could not resolve participant info offset.\n");
}
c_iterFree(topics);
if(result == U_RESULT_OK){
topics = u_participantFindTopic(participant, V_PUBLICATIONINFO_NAME, duration);
topic = c_iterTakeFirst(topics);
if(topic){
result = u_entityAction(u_entity(topic), resolveOffset, &publicationOffset);
} else {
result = U_RESULT_INTERNAL_ERROR;
in_printf(IN_LEVEL_SEVERE, "Could not resolve publication info offset.\n");
}
c_iterFree(topics);
}
if(result == U_RESULT_OK){
topics = u_participantFindTopic(participant, V_SUBSCRIPTIONINFO_NAME, duration);
topic = c_iterTakeFirst(topics);
if(topic){
result = u_entityAction(u_entity(topic), resolveOffset, &subscriptionOffset);
} else {
result = U_RESULT_INTERNAL_ERROR;
in_printf(IN_LEVEL_SEVERE, "Could not resolve subscription info offset.\n");
}
c_iterFree(topics);
}
if(result == U_RESULT_OK){
timeout.seconds = 0;
timeout.nanoseconds = 100 * 1000 * 1000; /*100 ms*/
in_printf(IN_LEVEL_FINE, "Collecting initial entities...\n");
result = handleParticipant(drset->participant_dr, participantOffset);
if(result == U_RESULT_OK){
result = handlePublication(drset->publication_dr, publicationOffset,
drset->participant_dr, participantOffset);
if(result == U_RESULT_OK){
result = handleSubscription(drset->subscription_dr, subscriptionOffset,
drset->participant_dr, participantOffset);
if(result == U_RESULT_OK){
vgroups = v_serviceTakeNewGroups(service);
vgroup = (v_group)c_iterTakeFirst(vgroups);
while(vgroup && result == U_RESULT_OK){
result = handleGroup(service, vgroup);
c_free(vgroup);
vgroup = (v_group)c_iterTakeFirst(vgroups);
}
c_iterFree(vgroups);
if(result == U_RESULT_OK){
in_printf(IN_LEVEL_FINE, "Waiting for entities to be created/deleted...\n");
} else {
in_printf(IN_LEVEL_SEVERE, "Could not collect initial groups...\n");
}
} else {
in_printf(IN_LEVEL_SEVERE, "Could not collect initial subscriptions...\n");
}
} else {
in_printf(IN_LEVEL_SEVERE, "Could not collect initial publications...\n");
}
} else {
in_printf(IN_LEVEL_SEVERE, "Could not collect initial participants...\n");
}
}
osr = os_threadAttrInit(&attr);
if(osr == os_resultSuccess){
osr = os_threadCreate(&clientWriterThread,
"clientWriterMonitor", &attr,
in_discoveryClientWriterMonitor, NULL);
if(osr != os_resultSuccess){
result = U_RESULT_INTERNAL_ERROR;
}
} else {
result = U_RESULT_INTERNAL_ERROR;
}
reportInterval = 0;
while(result == U_RESULT_OK && !terminate){
events = NULL;
/*Wait for events to occur*/
result = u_waitsetTimedWaitEvents(waitset, timeout, &events);
if(result == U_RESULT_OK){
event = (u_waitsetEvent)(c_iterTakeFirst(events));
while(event){
if(((event->events) & V_EVENT_DATA_AVAILABLE) ==
V_EVENT_DATA_AVAILABLE)
{
if(event->entity){
dataReader = (u_dataReader)event->entity;
gid = u_entityGid(event->entity);
if(v_gidCompare(gid, participantGid) == C_EQ){
result = handleParticipant(
drset->participant_dr, participantOffset);
} else if(v_gidCompare(gid, subscriptionGid) == C_EQ){
result = handleSubscription(
drset->subscription_dr, subscriptionOffset,
drset->participant_dr, participantOffset);
} else if(v_gidCompare(gid, publicationGid) == C_EQ){
result = handlePublication(
drset->publication_dr, publicationOffset,
drset->participant_dr, participantOffset);
} else {
in_printf(IN_LEVEL_SEVERE,
"This is impossible...at least in my understanding of the world.\n");
result = U_RESULT_INTERNAL_ERROR;
}
} else {
in_printf(IN_LEVEL_WARNING, "DATA_AVAILABLE (%d) but no entity.\n",
event->events);
}
} else if(((event->events) & V_EVENT_NEW_GROUP) ==
V_EVENT_NEW_GROUP)
{
vgroups = v_serviceTakeNewGroups(service);
vgroup = (v_group)c_iterTakeFirst(vgroups);
while(vgroup && result == U_RESULT_OK){
result = handleGroup(service, vgroup);
c_free(vgroup);
vgroup = (v_group)c_iterTakeFirst(vgroups);
}
c_iterFree(vgroups);
} else {
in_printf(IN_LEVEL_SEVERE, "Received unexpected event %d.\n", event->events);
result = U_RESULT_INTERNAL_ERROR;
}
u_waitsetEventFree(event);
event = (u_waitsetEvent)(c_iterTakeFirst(events));
}
} else if(result == U_RESULT_DETACHING){
in_printf(IN_LEVEL_INFO, "Starting termination now...\n");
} else if(result == U_RESULT_TIMEOUT){
result = U_RESULT_OK;
} else {
in_printf(IN_LEVEL_SEVERE, "Waitset wait failed.\n");
}
if(events){/* events may be null if waitset was deleted */
c_iterFree(events);
}
reportInterval++;
if(reportInterval >= 5){
/*reportEntities();*/
reportInterval = 0;
}
}
return result;
}
MStatus ik2Bsolver::doSolve()
//
// This is the doSolve method which calls solveIK.
//
{
MStatus stat;
// Handle Group
//
MIkHandleGroup * handle_group = handleGroup();
if (NULL == handle_group) {
return MS::kFailure;
}
// Handle
//
// For single chain types of solvers, get the 0th handle.
// Single chain solvers are solvers which act on one handle only,
// i.e. the handle group for a single chain solver
// has only one handle
//
MObject handle = handle_group->handle(0);
MDagPath handlePath = MDagPath::getAPathTo(handle);
MFnIkHandle handleFn(handlePath, &stat);
// Effector
//
MDagPath effectorPath;
handleFn.getEffector(effectorPath);
MFnIkEffector effectorFn(effectorPath);
// Mid Joint
//
effectorPath.pop();
MFnIkJoint midJointFn(effectorPath);
// Start Joint
//
MDagPath startJointPath;
handleFn.getStartJoint(startJointPath);
MFnIkJoint startJointFn(startJointPath);
// Preferred angles
//
double startJointPrefAngle[3];
double midJointPrefAngle[3];
startJointFn.getPreferedAngle(startJointPrefAngle);
midJointFn.getPreferedAngle(midJointPrefAngle);
// Set to preferred angles
//
startJointFn.setRotation(startJointPrefAngle,
startJointFn.rotationOrder());
midJointFn.setRotation(midJointPrefAngle,
midJointFn.rotationOrder());
AwPoint handlePos = handleFn.rotatePivot(MSpace::kWorld);
AwPoint effectorPos = effectorFn.rotatePivot(MSpace::kWorld);
AwPoint midJointPos = midJointFn.rotatePivot(MSpace::kWorld);
AwPoint startJointPos = startJointFn.rotatePivot(MSpace::kWorld);
AwVector poleVector = poleVectorFromHandle(handlePath);
poleVector *= handlePath.exclusiveMatrix();
double twistValue = twistFromHandle(handlePath);
AwQuaternion qStart, qMid;
solveIK(startJointPos,
midJointPos,
effectorPos,
handlePos,
poleVector,
twistValue,
qStart,
qMid);
midJointFn.rotateBy(qMid, MSpace::kWorld);
startJointFn.rotateBy(qStart, MSpace::kWorld);
return MS::kSuccess;
}
MStatus ik2Bsolver::doSolve()
//
// This is the doSolve method which calls solveIK.
//
{
MStatus stat;
// Handle Group
//
MIkHandleGroup * handle_group = handleGroup();
if (NULL == handle_group) {
return MS::kFailure;
}
// Handle
//
// For single chain types of solvers, get the 0th handle.
// Single chain solvers are solvers which act on one handle only,
// i.e. the handle group for a single chain solver
// has only one handle
//
MObject handle = handle_group->handle(0);
MDagPath handlePath = MDagPath::getAPathTo(handle);
MFnIkHandle handleFn(handlePath, &stat);
// Effector
//
MDagPath effectorPath;
handleFn.getEffector(effectorPath);
// MGlobal::displayInfo(effectorPath.fullPathName());
MFnIkEffector effectorFn(effectorPath);
// Mid Joint
//
effectorPath.pop();
MFnIkJoint midJointFn(effectorPath);
// End Joint
//
MDagPath endJointPath;
bool hasEndJ = findFirstJointChild(effectorPath, endJointPath);
// if(hasEndJ) MGlobal::displayInfo(endJointPath.fullPathName());
MFnIkJoint endJointFn(endJointPath);
// Start Joint
//
MDagPath startJointPath;
handleFn.getStartJoint(startJointPath);
MFnIkJoint startJointFn(startJointPath);
// Preferred angles
//
double startJointPrefAngle[3];
double midJointPrefAngle[3];
startJointFn.getPreferedAngle(startJointPrefAngle);
midJointFn.getPreferedAngle(midJointPrefAngle);
// Set to preferred angles
//
startJointFn.setRotation(startJointPrefAngle,
startJointFn.rotationOrder());
midJointFn.setRotation(midJointPrefAngle,
midJointFn.rotationOrder());
MPoint handlePos = handleFn.rotatePivot(MSpace::kWorld);
MPoint effectorPos = effectorFn.rotatePivot(MSpace::kWorld);
MPoint midJointPos = midJointFn.rotatePivot(MSpace::kWorld);
MPoint startJointPos = startJointFn.rotatePivot(MSpace::kWorld);
MVector poleVector = poleVectorFromHandle(handlePath);
poleVector *= handlePath.exclusiveMatrix();
double twistValue = twistFromHandle(handlePath);
MObject thisNode = thisMObject();
MVector localEndJointT = endJointFn.getTranslation(MSpace::kTransform);
MVector worldEndJointT = endJointFn.rotatePivot(MSpace::kWorld) - midJointPos;
double scaling = worldEndJointT.length() / ( localEndJointT.length() + 1e-6);
// MGlobal::displayInfo(MString(" scaling ")+scaling);
// get rest length
//
double restL1, restL2;
MPlug(thisNode, arestLength1).getValue(restL1);
MPlug(thisNode, arestLength2).getValue(restL2);
// get soft distance
//
MPlug plug( thisNode, asoftDistance );
double softD = 0.0;
plug.getValue(softD);
restL1 *= scaling;
restL2 *= scaling;
softD *= scaling;
// get max stretching
double maxStretching = 0.0;
MPlug(thisNode, amaxStretching).getValue(maxStretching);
MQuaternion qStart, qMid;
double stretching = 0.0;
solveIK(startJointPos,
midJointPos,
effectorPos,
handlePos,
poleVector,
twistValue,
qStart,
qMid,
softD,
restL1, restL2,
stretching);
midJointFn.rotateBy(qMid, MSpace::kWorld);
startJointFn.rotateBy(qStart, MSpace::kWorld);
midJointFn.setTranslation(MVector(restL1 / scaling, 0.0, 0.0), MSpace::kTransform);
endJointFn.setTranslation(MVector(restL2 / scaling, 0.0, 0.0), MSpace::kTransform);
// if(stretching > maxStretching) stretching = maxStretching;
if(maxStretching > 0.0) {
MVector vstretch(stretching* 0.5 / scaling, 0.0, 0.0);
midJointFn.translateBy(vstretch, MSpace::kTransform);
endJointFn.translateBy(vstretch, MSpace::kTransform);
}
return MS::kSuccess;
}
MStatus splineSolverNode::preSolve()
{
MStatus stat;
setRotatePlane(false);
setSingleChainOnly(true);
setPositionOnly(false);
//Get Handle
MIkHandleGroup * handle_group = handleGroup();
if (NULL == handle_group) {
return MS::kFailure;
}
MObject handle = handle_group->handle( 0 );
MDagPath handlePath = MDagPath::getAPathTo( handle );
fnHandle.setObject( handlePath );
//Get Curve
MPlug inCurvePlug = fnHandle.findPlug( "inCurve" );
MDataHandle curveHandle = inCurvePlug.asMDataHandle();
MObject inputCurveObject = curveHandle.asNurbsCurveTransformed();
curveFn.setObject( inputCurveObject );
float initCurveLength = curveFn.length();
MVector initNormal = curveFn.normal(0);
MVector initTangent = curveFn.tangent(0);
float stretchRatio = 1;
// Get the position of the end_effector
//
MDagPath effectorPath;
fnHandle.getEffector(effectorPath);
tran.setObject( effectorPath );
// Get the start joint position
//
MDagPath startJointPath;
fnHandle.getStartJoint( startJointPath );
joints.clear();
//Get Joints
while (true)
{
effectorPath.pop();
joints.push_back( effectorPath );
if (effectorPath == startJointPath)
break;
}
std::reverse(joints.begin(), joints.end());
if (!fnHandle.hasAttribute("str"))
{
//Add Custom Attributes to Handle
MFnNumericAttribute fnAttr;
MObject attr = fnAttr.create("stretchRatio", "str", MFnNumericData::kDouble, stretchRatio);
fnAttr.setKeyable(1);
fnAttr.setWritable(1);
fnAttr.setMin(0);
fnAttr.setMax(1);
fnAttr.setHidden(0);
fnAttr.setStorable(1);
fnAttr.setReadable(1);
fnHandle.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr);
attr = fnAttr.create("anchorPosition", "ancp", MFnNumericData::kDouble, 0.0);
fnAttr.setKeyable(1);
fnAttr.setWritable(1);
fnAttr.setMin(0);
fnAttr.setMax(1);
fnAttr.setHidden(0);
fnAttr.setStorable(1);
fnAttr.setReadable(1);
fnHandle.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr);
attr = fnAttr.create("curveLength", "cvLen", MFnNumericData::kDouble, initCurveLength);
fnAttr.setKeyable(0);
fnAttr.setWritable(1);
fnAttr.setHidden(1);
fnAttr.setStorable(1);
fnAttr.setReadable(1);
fnHandle.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr);
attr = fnAttr.create("initNormal", "norm", MFnNumericData::k3Double);
fnAttr.setDefault(initNormal.x, initNormal.y, initNormal.z);
fnAttr.setKeyable(0);
fnAttr.setWritable(1);
fnAttr.setHidden(1);
fnAttr.setStorable(1);
fnAttr.setReadable(1);
fnHandle.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr);
attr = fnAttr.create("initTangent", "tang", MFnNumericData::k3Double);
fnAttr.setDefault(initTangent.x, initTangent.y, initTangent.z);
fnAttr.setKeyable(0);
fnAttr.setWritable(1);
fnAttr.setHidden(1);
fnAttr.setStorable(1);
fnAttr.setReadable(1);
fnHandle.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr);
attr = fnAttr.create("jointsLength", "jsLen", MFnNumericData::kDouble, getJointsTotalLenght());
fnAttr.setKeyable(0);
fnAttr.setWritable(1);
fnAttr.setHidden(1);
fnAttr.setStorable(1);
fnAttr.setReadable(1);
fnHandle.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr);
attr = fnAttr.create("startTwist", "strtw", MFnNumericData::kDouble, 0.0);
fnAttr.setKeyable(1);
fnAttr.setWritable(1);
fnAttr.setHidden(0);
fnAttr.setStorable(1);
fnAttr.setReadable(1);
fnHandle.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr);
attr = fnAttr.create("endTwist", "endtw", MFnNumericData::kDouble, 0.0);
fnAttr.setKeyable(1);
fnAttr.setWritable(1);
fnAttr.setHidden(0);
fnAttr.setStorable(1);
fnAttr.setReadable(1);
fnHandle.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr);
MObject twistRamp = MRampAttribute::createCurveRamp("twistRamp", "twr");
fnHandle.addAttribute(twistRamp, MFnDependencyNode::kLocalDynamicAttr);
MObject scaleRamp = MRampAttribute::createCurveRamp("scaleRamp", "scr");
fnHandle.addAttribute(scaleRamp, MFnDependencyNode::kLocalDynamicAttr);
} else
{
MPlug strPlug = fnHandle.findPlug("str");
stretchRatio = strPlug.asDouble();
}
return MS::kSuccess;
}
MStatus CIKSolverNode::doSimpleSolver()
{
MStatus stat;
// Get the handle and create a function set for it
//
MIkHandleGroup* handle_group = handleGroup();
if (NULL == handle_group) {
return MS::kFailure;
}
MObject handle = handle_group->handle(0);
MDagPath handlePath = MDagPath::getAPathTo(handle);
MFnIkHandle fnHandle(handlePath, &stat);
// End-Effector
MDagPath endEffectorPath;
fnHandle.getEffector(endEffectorPath);
MFnIkEffector fnEffector(endEffectorPath);
MPoint effectorPos = fnEffector.rotatePivot(MSpace::kWorld);
unsigned int numJoints = endEffectorPath.length();
std::vector<MDagPath> jointsDagPaths; jointsDagPaths.reserve(numJoints);
while (endEffectorPath.length() > 1)
{
endEffectorPath.pop();
jointsDagPaths.push_back( endEffectorPath );
}
std::reverse(jointsDagPaths.begin(), jointsDagPaths.end());
static bool builtLocalSkeleton = false;
if (builtLocalSkeleton == false)
{
for (int jointIdx = 0; jointIdx < jointsDagPaths.size(); ++jointIdx)
{
MFnIkJoint curJoint(jointsDagPaths[jointIdx]);
m_localJointsPos.push_back( curJoint.getTranslation(MSpace::kWorld) );
}
m_localJointsPos.push_back(effectorPos );
builtLocalSkeleton = true;
}
MPoint startJointPos = MFnIkJoint(jointsDagPaths.front()).getTranslation(MSpace::kWorld);
MVector startToEndEff = m_localJointsPos.back() - m_localJointsPos.front();
double curveLength = (getPosition(1.0) - getPosition(0.0)).length();
double chainLength = startToEndEff.length(); // in local space.
double stretchFactor = curveLength / chainLength;
double uVal = 0.0f;
MVector jointPosL = m_localJointsPos[0];
for (int jointIdx = 0; jointIdx < jointsDagPaths.size(); ++jointIdx)
{
MFnIkJoint curJoint(jointsDagPaths[jointIdx]);
MVector curJointPosL = m_localJointsPos[jointIdx];
double dist = stretchFactor * (curJointPosL - jointPosL).length();
uVal = uVal + dist / curveLength;
MVector curCurveJointPos = getPosition(uVal);
curJoint.setTranslation(curCurveJointPos, MSpace::kWorld);
jointPosL = curJointPosL;
}
MVector effectorCurvePos = getPosition(1.0);
MVector curCurveJointPos = getPosition(uVal);
MVector effectorVec = (effectorCurvePos - curCurveJointPos).normal();
double endJointAngle[3];
MVector effectorVecXY = MVector(effectorVec(0), effectorVec(1), 0.0);
endJointAngle[2] = effectorVecXY.angle(MVector(1, 0, 0));
if ((MVector(1, 0, 0) ^ effectorVecXY) * MVector(0, 0, 1) < 0.0) { endJointAngle[2] = -endJointAngle[2]; }
MVector effectorVecXZ = MVector(effectorVec(0), 0.0, effectorVec(2));
endJointAngle[1] = effectorVecXZ.angle(MVector(1, 0, 0));
if ((MVector(1, 0, 0) ^ effectorVecXZ) * MVector(0, 1, 0) < 0.0) { endJointAngle[1] = -endJointAngle[1]; }
endJointAngle[0] = 0.0;
MFnIkJoint curJoint(jointsDagPaths.back()); curJoint.setRotation(endJointAngle, curJoint.rotationOrder());
return MS::kSuccess;
}
