void CorbaTest::testPortBufferConnection()
{
// This test assumes that there is a buffer connection mo => mi of size 3
// Check if connection succeeded both ways:
BOOST_CHECK( mo->connected() );
BOOST_CHECK( mi->connected() );
double value = 0;
// Check if no-data works
BOOST_CHECK_EQUAL( mi->read(value), NoData );
// Check if writing works
ASSERT_PORT_SIGNALLING(mo->write(1.0), mi);
ASSERT_PORT_SIGNALLING(mo->write(2.0), mi);
ASSERT_PORT_SIGNALLING(mo->write(3.0), mi);
ASSERT_PORT_SIGNALLING(mo->write(4.0), 0);
BOOST_CHECK( mi->read(value) );
BOOST_CHECK_EQUAL( 1.0, value );
BOOST_CHECK( mi->read(value) );
BOOST_CHECK_EQUAL( 2.0, value );
BOOST_CHECK( mi->read(value) );
BOOST_CHECK_EQUAL( 3.0, value );
BOOST_CHECK_EQUAL( mi->read(value), OldData );
}
void CorbaMQueueIPCTest::testPortBufferConnection()
{
// This test assumes that there is a buffer connection mw1 => server => mr1 of size 3
// Check if connection succeeded both ways:
BOOST_CHECK( mw1->connected() );
BOOST_CHECK( mr1->connected() );
double value = 0;
// Check if no-data works
BOOST_CHECK( !mr1->read(value) );
// Check if writing works
ASSERT_PORT_SIGNALLING(mw1->write(1.0), mr1);
ASSERT_PORT_SIGNALLING(mw1->write(2.0), mr1);
ASSERT_PORT_SIGNALLING(mw1->write(3.0), mr1);
// it will be emptied too fast by mqueue.
//ASSERT_PORT_SIGNALLING(mw1->write(4.0), 0);
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 1.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 2.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 3.0, value );
BOOST_CHECK_EQUAL( mr1->read(value), OldData );
}
void CorbaMQueueIPCTest::testPortBufferConnection()
{
// This test assumes that there is a buffer connection mw1 => server => mr1 of size 3
// Check if connection succeeded both ways:
BOOST_CHECK( mw1->connected() );
BOOST_CHECK( mr1->connected() );
double value = 0;
// Check if no-data works
BOOST_CHECK( !mr1->read(value) );
// Check if writing works
ASSERT_PORT_SIGNALLING(mw1->write(1.0), mr1);
ASSERT_PORT_SIGNALLING(mw1->write(2.0), mr1);
ASSERT_PORT_SIGNALLING(mw1->write(3.0), mr1);
// There are two connections between mw1 and mr1. We first flush one of the
// two, and then the second one (normal multi-writer single-reader
// behaviour)
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 1.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 2.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 3.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 1.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 2.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 3.0, value );
BOOST_CHECK_EQUAL( mr1->read(value), OldData );
}
void updateHook() {
dummyInPort.read(dataVar);
dummyOutPort.write(dataVar);
dataInPort.read(dataVar);
dataOutPort.write(dataVar);
//log(Info) << "Worker relaying data!" << endlog();
}
// Update =====================================================
CStatus gInvRotorderOp_Update( CRef& in_ctxt )
{
OperatorContext ctxt( in_ctxt );
// Inputs --------------------------------------------------------------
long iRotorder = ctxt.GetParameterValue(L"Rotorder");
long iOut = 0;
if(iRotorder == 0)
iOut = 5;
else if (iRotorder == 1)
iOut = 3;
else if (iRotorder == 2)
iOut = 4;
else if (iRotorder == 3)
iOut = 1;
else if (iRotorder == 4)
iOut = 2;
else if (iRotorder == 5)
iOut = 0;
OutputPort Out = ctxt.GetOutputPort();
Out.PutValue(iOut);
return CStatus::OK;
}
// Update =====================================================
CStatus sn_inverseRotorder_op_Update( CRef& in_ctxt )
{
OperatorContext ctxt( in_ctxt );
LONG rotorder = ctxt.GetParameterValue(L"rotorder");
LONG out_ro = 0;
if(rotorder == 0)
out_ro = 5;
else if (rotorder == 1)
out_ro = 3;
else if (rotorder == 2)
out_ro = 4;
else if (rotorder == 3)
out_ro = 1;
else if (rotorder == 4)
out_ro = 2;
else if (rotorder == 5)
out_ro = 0;
OutputPort Out = ctxt.GetOutputPort();
Out.PutValue(out_ro);
return CStatus::OK;
}
void updateHook() {
cwport.write(i);
swport.write(i);
iwport.write(i);
fwport.write(i);
dwport.write( std::vector<double>(10,i) );
++i;
}
wxString
InputPort::GetType() const
{
OutputPort *output = GetSource();
if (output != NULL)
{
return output->GetType();
}
return wxT("?");
}
OutputPort* NodeInspector::outputPort(const Datum* d) const
{
for (auto row : rows)
for (auto a : row->childItems())
{
OutputPort* p = dynamic_cast<OutputPort*>(a);
if (p && p->getDatum() == d)
return p;
}
return NULL;
}
virtual void disconnect(bool forward)
{
// Call the base class first
base::ChannelElement<T>::disconnect(forward);
OutputPort<T>* port = this->port;
if (port && !forward)
{
this->port = 0;
port->removeConnection( cid );
}
}
TestTaskContext(std::string name)
: TaskContext(name),
hello("Hello", "The hello thing", "World"),
cwport("cw_port", 'a'),
swport("sw_port", 1),
iwport("iw_port", 0),
fwport("fw_port", 0.0),
dwport("dw_port"),
crport("cr_port"),
srport("sr_port"),
irport("ir_port"),
frport("fr_port"),
drport("dr_port"), i(0)
{
this->properties()->addProperty( hello );
this->ports()->addPort( cwport );
this->ports()->addPort( swport );
this->ports()->addPort( iwport );
this->ports()->addPort( fwport );
this->ports()->addPort( dwport );
this->ports()->addPort( crport );
this->ports()->addPort( srport );
this->ports()->addPort( irport );
this->ports()->addPort( frport );
this->ports()->addPort( drport );
// write initial value.
std::vector<double> init(10, 5.4528);
dwport.write( init );
}
void updateHook() {
double input_sample;
if ( input.read(input_sample) ) {
current += this->engine()->getActivity()->getPeriod()*input_sample /inertia.value();
}
output.write( current );
}
static bool createConnection(OutputPort<T>& output_port, base::InputPortInterface& input_port, ConnPolicy const& policy)
{
if ( !output_port.isLocal() ) {
log(Error) << "Need a local OutputPort to create connections." <<endlog();
return false;
}
InputPort<T>* input_p = dynamic_cast<InputPort<T>*>(&input_port);
// This is the input channel element of the output half
base::ChannelElementBase::shared_ptr output_half = 0;
if (input_port.isLocal() && policy.transport == 0)
{
// Local connection
if (!input_p)
{
log(Error) << "Port " << input_port.getName() << " is not compatible with " << output_port.getName() << endlog();
return false;
}
// local ports, create buffer here.
output_half = buildBufferedChannelOutput<T>(*input_p, output_port.getPortID(), policy, output_port.getLastWrittenValue());
}
else
{
// if the input is not local, this is a pure remote connection,
// if the input *is* local, the user requested to use a different transport
// than plain memory, rare case, but we accept it. The unit tests use this for example
// to test the OOB transports.
if ( !input_port.isLocal() ) {
output_half = createRemoteConnection( output_port, input_port, policy);
} else
output_half = createOutOfBandConnection<T>( output_port, *input_p, policy);
}
if (!output_half)
return false;
// Since output is local, buildChannelInput is local as well.
// This this the input channel element of the whole connection
base::ChannelElementBase::shared_ptr channel_input =
buildChannelInput<T>(output_port, input_port.getPortID(), output_half);
return createAndCheckConnection(output_port, input_port, channel_input, policy );
}
bool configureHook() {
object_coordinates_Pose.setOrigin( tf::Vector3(1,2, 0.0) );
object_coordinates_Pose.setRotation( tf::createQuaternionFromRPY(0.75, 0, 0) );
object_PoseTF = geometric_semantics::Pose<tf::Pose> (object_PoseCoordinatesSemantics, object_coordinates_Pose);
object_PoseTF_msg = conversions_geometric_msgs::PoseTFToMsg(object_PoseTF);
outPortPose.setDataSample(object_PoseTF_msg);
log(Debug) << "Geometric_semantics_component_tutorial_publisher configured !" <<endlog();
return true;
}
void ArmBridgeRosOrocos::writeJointPositionsToPort(brics_actuator::JointPositions brics_joint_positions, std_msgs::Float64MultiArray& orocos_data_array, OutputPort<std_msgs::Float64MultiArray>& output_port)
{
for (size_t i = 0; i < brics_joint_positions.positions.size(); i++)
{
std::cout << brics_joint_positions.positions[i].value << " ";
orocos_data_array.data[i] = brics_joint_positions.positions[i].value;
}
output_port.write(orocos_data_array);
}
void updateHook() {
xpos = xpos + 0.001;
ypos = ypos + 0.001;
zpos = zpos + 0.000;
object_coordinates_Pose.setOrigin( tf::Vector3(xpos,ypos,zpos) );
object_PoseTF = geometric_semantics::Pose<tf::Pose> (object_PoseCoordinatesSemantics, object_coordinates_Pose);
object_PoseTF_msg = conversions_geometric_msgs::PoseTFToMsg(object_PoseTF);
outPortPose.write(object_PoseTF_msg);
log(Debug) << "Geometric_semantics_component_tutorial_publisher executes updateHook !" <<endlog();
}
void CorbaTest::testPortDataConnection()
{
// This test assumes that there is a data connection mo1 => mi2
// Check if connection succeeded both ways:
BOOST_CHECK( mo1->connected() );
BOOST_CHECK( mi2->connected() );
double value = 0;
// Check if no-data works
BOOST_CHECK_EQUAL( mi2->read(value), NoData );
// Check if writing works (including signalling)
ASSERT_PORT_SIGNALLING(mo1->write(1.0), mi2)
BOOST_CHECK( mi2->read(value) );
BOOST_CHECK_EQUAL( 1.0, value );
ASSERT_PORT_SIGNALLING(mo1->write(2.0), mi2);
BOOST_CHECK( mi2->read(value) );
BOOST_CHECK_EQUAL( 2.0, value );
}
int
InputPort::LuaGetSource(lua_State *L)
{
ShaderObject *obj = *(ShaderObject **)lua_touserdata(L, lua_upvalueindex(1));
InputPort *input = dynamic_cast<InputPort *>(obj);
OutputPort *source = input->GetSource();
if (source != NULL)
{
source->PushLua(L);
}
else
{
lua_newtable(L);
lua_pushcfunction(L, ::LuaUID);
lua_setfield(L, -2, "uid");
lua_pushcfunction(L, ::LuaGetType);
lua_setfield(L, -2, "get_type");
}
return 1;
}
void CorbaMQueueIPCTest::testPortDataConnection()
{
// This test assumes that there is a data connection mw1 => server => mr1
// Check if connection succeeded both ways:
BOOST_CHECK( mw1->connected() );
BOOST_CHECK( mr1->connected() );
double value = 0;
// Check if no-data works
BOOST_CHECK( !mr1->read(value) );
// Check if writing works (including signalling)
ASSERT_PORT_SIGNALLING(mw1->write(1.0), mr1)
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 1.0, value );
ASSERT_PORT_SIGNALLING(mw1->write(2.0), mr1);
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 2.0, value );
}
TestTaskContext(std::string name)
: RTT::TaskContext(name),
hello("Hello", "The hello thing", "World"),
dwport("D2Port"),
drport("D1Port")
{
this->properties()->addProperty( hello );
this->ports()->addPort( drport );
this->ports()->addPort( dwport );
std::vector<double> init(10, 1.0);
dwport.write(init);
}
TestTaskContext2(std::string name)
: RTT::TaskContext(name),
hello("Hello", "The hello thing", "World"),
dwport("D1Port"),
drport("D2Port")
{
this->properties()->addProperty( hello );
this->ports()->addPort( drport );
this->ports()->addPort( dwport );
// write initial value.
dwport.write( 0.0 );
}
///////////////////////////////////////////////////////////////
// CURVE LENGTH
///////////////////////////////////////////////////////////////
// Define =====================================================
XSIPLUGINCALLBACK CStatus sn_curvelength_op_Update( CRef& in_ctxt )
{
OperatorContext ctxt( in_ctxt );
// Inputs -----------------------------------------
Primitive crvprim(ctxt.GetInputValue(0));
NurbsCurveList crvlist(crvprim.GetGeometry());
NurbsCurve crv(crvlist.GetCurves().GetItem(0));
// KinematicState kCurve(ctxt.GetInputValue(2));
// CTransformation tCurve(kCurve.GetTransform());
// ------------------------------------------------
//CVector3 vScale(tCurve.GetScaling());
double length;
crv.GetLength(length);
OutputPort outPort = ctxt.GetOutputPort();
outPort.PutValue(length);
return CStatus::OK;
}
TestTaskContext(std::string name)
: TaskContext(name),
hello("Hello", "The hello thing", "World"),
dwport("D2Port"),
drport("D1Port"),
init(10,1.0)
{
this->properties()->addProperty( hello );
this->ports()->addPort( drport );
this->ports()->addPort( dwport );
pos = 10;
Logger::log() << Logger::Info << "TestTaskContext initialized" << Logger::endl;
// write initial value.
dwport.setDataSample( init );
}
void updateHook()
{
if(NewData==posInc_in.read(posIncData) && NewData==deltaInc_in.read(deltaIncData))
{
/*for(int i=0; i<number_of_drives; i++)
{
std::cout << "POS INC" << posIncData[i] << std::endl;
}*/
computedPwm_out.write(computePwmValue(posIncData,deltaIncData));
}
/*else
std::cout << "NO DATA" << std::endl;
*/
}
virtual void updateHook () {
if( pos > 9 )
{
init[2]+=2;
} else if( pos > 3 ) {
init[2]++;
} else {
init[2]--;
}
if( pos > 5 ) {
init[4] += 4;
} else {
init[4] -= 4;
}
pos--;
if( pos == 0 ) { pos = 10; }
dwport.write( init );
}
CStatus CHelperBoneOp::Update
(
UpdateContext& ctx,
OutputPort& output
)
{
///////////////////////////////////////////////////////////////
// get operator
///////////////////////////////////////////////////////////////
Operator op(ctx.GetOperator());
///////////////////////////////////////////////////////////////
// get output port
///////////////////////////////////////////////////////////////
KinematicState gkHelper(output.GetValue());
///////////////////////////////////////////////////////////////
// get helper bone data
///////////////////////////////////////////////////////////////
InputPort bonedataport(op.GetPort(L"bonedataport",L"HelperBoneGroup",0));
Property HelperBoneData(bonedataport.GetValue());
bool enabled = HelperBoneData.GetParameterValue(L"Enabled");
// not enabled: do nothing
if (!enabled)
return CStatus::OK;
///////////////////////////////////////////////////////////////
// evaluate new transformation for helperbone
///////////////////////////////////////////////////////////////
XSI::MATH::CTransformation tNewPose;
bool bComputeInWorldSpace = (0!=(long)HelperBoneData.GetParameterValue(L"ComputationSpace"));
///////////////////////////////////////////////////////////////
// get objects connected to input & output ports
///////////////////////////////////////////////////////////////
InputPort rootboneport(op.GetPort(L"globalkineport",L"RootBoneGroup",0));
InputPort parentboneport(op.GetPort(L"globalkineport",L"ParentBoneGroup",0));
InputPort childboneport(op.GetPort(L"globalkineport",L"ChildBoneGroup",0));
KinematicState gkRoot(rootboneport.GetValue());
KinematicState gkParent(parentboneport.GetValue());
KinematicState gkChild(childboneport.GetValue());
// get helperbonedata values
XSI::MATH::CVector3 vBasePos(
HelperBoneData.GetParameterValue(L"BoneOffsetX"),
HelperBoneData.GetParameterValue(L"BoneOffsetY"),
HelperBoneData.GetParameterValue(L"BoneOffsetZ") );
double perc_along_root = (double)HelperBoneData.GetParameterValue(L"BoneDistance") / 100.0;
double root_bone_length = (double)HelperBoneData.GetParameterValue(L"RootBoneLength");
XSI::GridData griddata(HelperBoneData.GetParameterValue(L"Triggers"));
// read triggers
ReadTriggerData(griddata);
// GET TRANSFORMATIONS OF ROOT, PARENT & CHILD
CTransformation tGRoot = gkRoot.GetTransform();
CTransformation tGParent = gkParent.GetTransform();
CTransformation tGChild = gkChild.GetTransform();
///////////////////////////////////////////////////////////////
// compute new orientation and position based on triggers
///////////////////////////////////////////////////////////////
DebugPrint( L"parent", tGParent.GetRotationQuaternion() );
DebugPrint( L"child", tGChild.GetRotationQuaternion() );
// GET LOCAL TRANSFORM OF CHILD RELATIVE TO PARENT
XSI::MATH::CMatrix3 m3Parent( tGParent.GetRotationMatrix3() );
DebugPrint(L"m3Parent->", m3Parent);
XSI::MATH::CMatrix3 m3Child( tGChild.GetRotationMatrix3() );
DebugPrint(L"m3Parent->", m3Child);
m3Parent.TransposeInPlace();
DebugPrint(L"m3Parent.TransposeInPlace->", m3Parent);
// bug #90494
// m3Child.MulInPlace( m3Parent );
MulInPlace( m3Child, m3Parent );
DebugPrint(L"m3Child.MulInPlace->", m3Child);
// GET ORIENTATION OF BONE2 RELATIVE TO BONE1 AS A QUATERNION
XSI::MATH::CQuaternion qBone2 = m3Child.GetQuaternion();
DebugPrint( L"child2parent", qBone2 );
// MATCH QUATERNIONS
ComputeWeights( qBone2, m_cTriggers, m_aEnabled, m_aWeights, m_aTriggerOri, m_aTriggerTol );
// SUM TARGET ORIENTATIONS & POSITIONS
XSI::MATH::CQuaternion qNewOri;
XSI::MATH::CVector3 vNewPos;
SumTargets( qNewOri, vNewPos, m_cTriggers, m_aWeights, m_aTargetOri, m_aTargetPos );
DebugPrint( L"qNewPos->", qNewPos );
DebugPrint( L"vNewOri->", qNewOri );
// UPDATE TRANSFORMATION
if (bComputeInWorldSpace)
{
// not implemented
}
else // Root object space
{
// compute initial helperbone position
vBasePos.PutX( vBasePos.GetX() + (root_bone_length * perc_along_root) );
vNewPos.AddInPlace(vBasePos);
// apply changes from triggers
tNewPose.SetRotationFromQuaternion( qNewOri );
tNewPose.SetTranslation( vNewPos );
// map root object space to worldspace
tNewPose.MulInPlace( tGRoot );
}
///////////////////////////////////////////////////////////////
// update output port
///////////////////////////////////////////////////////////////
gkHelper.PutTransform( tNewPose );
return CStatus::OK;
}
static base::ChannelElementBase::shared_ptr buildBufferedChannelInput(OutputPort<T>& port, ConnID* conn_id, ConnPolicy const& policy, base::ChannelElementBase::shared_ptr output_channel)
{
assert(conn_id);
base::ChannelElementBase::shared_ptr endpoint = new ConnInputEndpoint<T>(&port, conn_id);
base::ChannelElementBase::shared_ptr data_object = buildDataStorage<T>(policy, port.getLastWrittenValue() );
endpoint->setOutput(data_object);
if (output_channel)
data_object->setOutput(output_channel);
return endpoint;
}
XSI::CStatus CAxisInterpOp::Update
(
UpdateContext& ctx,
OutputPort& output
)
{
Operator op(ctx.GetOperator());
///////////////////////////////////////////////////////////////
// get operator parameters
///////////////////////////////////////////////////////////////
XSI::CString triggers(op.GetParameterValue(L"Triggers"));
// triggers changed
if ( m_csTriggers != triggers )
{
m_csTriggers = triggers;
Init( ctx, 0 );
}
double boneperc = op.GetParameterValue(L"BoneDist");
///////////////////////////////////////////////////////////////
// get objects connected to input & output ports
///////////////////////////////////////////////////////////////
InputPort rootboneport(op.GetPort(L"globalkineport",L"RootBoneGroup",0));
InputPort parentboneport(op.GetPort(L"globalkineport",L"ParentBoneGroup",0));
InputPort parentbonelenport(op.GetPort(L"bonelengthport",L"ParentBoneGroup",0));
InputPort childboneport(op.GetPort(L"globalkineport",L"ChildBoneGroup",0));
KinematicState gkRoot(rootboneport.GetValue());
KinematicState gkParent(parentboneport.GetValue());
double parentbonelen(parentbonelenport.GetValue());
KinematicState gkChild(childboneport.GetValue());
KinematicState gkHelper(output.GetValue());
// GET TRANSFORMATIONS OF ROOT, PARENT & CHILD
CTransformation tGRoot = gkRoot.GetTransform();
CTransformation tGBone1 = gkParent.GetTransform();
CTransformation tGBone2 = gkChild.GetTransform();
#ifdef _DEBUG_UPDATE
{
Application app;
wchar_t wszBuf[256];
XSI::MATH::CQuaternion q = tGBone1.GetRotationQuaternion();
double x, y, z;
q.GetXYZAnglesValues(x,y,z);
swprintf( wszBuf, L"parent R(%f,%f,%f)]", r2d(x),r2d(y),r2d(z) );
app.LogMessage( (const wchar_t*)wszBuf );
}
#endif
#ifdef _DEBUG_UPDATE
{
Application app;
wchar_t wszBuf[256];
XSI::MATH::CQuaternion q = tGBone2.GetRotationQuaternion();
double x, y, z;
q.GetXYZAnglesValues(x,y,z);
swprintf( wszBuf, L"child R(%f,%f,%f)]", r2d(x),r2d(y),r2d(z) );
app.LogMessage( (const wchar_t*)wszBuf );
}
#endif
///////////////////////////////////////////////////////////////
// perform update function
///////////////////////////////////////////////////////////////
// GET LOCAL TRANSFORM OF CHILD RELATIVE TO PARENT
XSI::MATH::CMatrix3 mBone1( tGBone1.GetRotationMatrix3() );
XSI::MATH::CMatrix3 mBone2( tGBone2.GetRotationMatrix3() );
#ifdef _DEBUG_UPDATE
{
Application app;
wchar_t wszBuf[256];
double m0, m1, m2, m3, m4, m5, m6, m7, m8;
mBone1.Get(m0, m1, m2, m3, m4, m5, m6, m7, m8);
swprintf( wszBuf, L"mBone1->\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f", m0, m1, m2, m3, m4, m5, m6, m7, m8 );
app.LogMessage( (const wchar_t*)wszBuf );
}
#endif
#ifdef _DEBUG_UPDATE
{
Application app;
wchar_t wszBuf[256];
double m0, m1, m2, m3, m4, m5, m6, m7, m8;
mBone2.Get(m0, m1, m2, m3, m4, m5, m6, m7, m8);
swprintf( wszBuf, L"mBone2->\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f", m0, m1, m2, m3, m4, m5, m6, m7, m8 );
app.LogMessage( (const wchar_t*)wszBuf );
}
#endif
mBone1.TransposeInPlace();
#ifdef _DEBUG_UPDATE
{
Application app;
wchar_t wszBuf[256];
double m0, m1, m2, m3, m4, m5, m6, m7, m8;
mBone1.Get(m0, m1, m2, m3, m4, m5, m6, m7, m8);
swprintf( wszBuf, L"mBone1.TransposeInPlace->\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f", m0, m1, m2, m3, m4, m5, m6, m7, m8 );
app.LogMessage( (const wchar_t*)wszBuf );
}
#endif
XSI::MATH::CMatrix3 tmpMat3;
for(int nR=0; nR<3; nR++)
for(int nC=0; nC<3; nC++)
{
tmpMat3.SetValue(nR,nC ,
mBone2.GetValue(nR,0) * mBone1.GetValue(0,nC) +
mBone2.GetValue(nR,1) * mBone1.GetValue(1,nC) +
mBone2.GetValue(nR,2) * mBone1.GetValue(2,nC) );
}
mBone2 = tmpMat3;
// bug #90494
// mBone2.MulInPlace( mBone1 );
#ifdef _DEBUG_UPDATE
{
Application app;
wchar_t wszBuf[256];
double m0, m1, m2, m3, m4, m5, m6, m7, m8;
mBone2.Get(m0, m1, m2, m3, m4, m5, m6, m7, m8);
swprintf( wszBuf, L"mBone2.MulInPlace( mBone1 )->\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f", m0, m1, m2, m3, m4, m5, m6, m7, m8 );
app.LogMessage( (const wchar_t*)wszBuf );
}
#endif
// GET ORIENTATION OF BONE2 RELATIVE TO BONE1 AS A QUATERNION
XSI::MATH::CQuaternion qBone2 = mBone2.GetQuaternion();
// MATCH QUATERNIONS
XSI::MATH::CVector3 vBasePos(
op.GetParameterValue(L"BasePoseX"),
op.GetParameterValue(L"BasePoseY"),
op.GetParameterValue(L"BasePoseZ") );
#ifdef _DEBUG_UPDATE
{
Application app;
wchar_t wszBuf[256];
double x, y, z;
qBone2.GetXYZAnglesValues(x,y,z);
swprintf( wszBuf, L"child2parent R(%f,%f,%f)]", r2d(x),r2d(y),r2d(z) );
app.LogMessage( (const wchar_t*)wszBuf );
}
#endif
ComputeBaseOffset(vBasePos, boneperc, parentbonelen );
ComputeWeights( qBone2, m_cTriggers, m_aWeights, m_aTriggerOri, m_aTriggerTol );
// SUM TARGET ORIENTATIONS & POSITIONS
XSI::MATH::CQuaternion qNewOri;
SumTargets( qNewOri, vBasePos, m_cTriggers, m_aWeights, m_aTargetOri, m_aTargetPos );
#ifdef _DEBUG_UPDATE
Application app;
wchar_t wszBuf[256];
double x, y, z;
qNewOri.GetXYZAnglesValues(x,y,z);
swprintf( wszBuf, L"qNewOri->R(%f,%f,%f)]", r2d(x),r2d(y),r2d(z) );
app.LogMessage( (const wchar_t*)wszBuf );
#endif
// UPDATE TRANSFORMATION
XSI::MATH::CTransformation tNewPose;
tNewPose.SetRotationFromQuaternion( qNewOri );
tNewPose.SetTranslation( vBasePos );
tNewPose.MulInPlace( tGRoot );
///////////////////////////////////////////////////////////////
// update output port
///////////////////////////////////////////////////////////////
gkHelper.PutTransform( tNewPose );
return CStatus::OK;
}
bool configureHook() {
dataOutPort.setDataSample(dataVar);
dummyOutPort.setDataSample(dataVar);
//std::cout << "Orocos_process_spam configured !" << std::endl;
return true;
}
void CorbaTest::testPortDisconnected()
{
BOOST_CHECK( !mo1->connected() );
BOOST_CHECK( !mi2->connected() );
}
