00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00042
00043
00044
00045 #include "../tool/disable4786.h"
00046
00047 #include <stdlib.h>
00048 #include "CommonNodeFactory.h"
00049 #include "MergeNode.h"
00050 #include "TriangulateOrientationNode.h"
00051 #include "VirtualTransformation.h"
00052 #include "DynamicTransformation.h"
00053 #include "InvertTransformation.h"
00054 #include "MatrixTransformation.h"
00055 #include "SelectionNode.h"
00056 #include "ConfidenceFilterNode.h"
00057 #include "ConfidenceSelectNode.h"
00058 #include "FilterNode.h"
00059 #include "ThresholdFilterNode.h"
00060 #include "ButtonFilterNode.h"
00061 #include "ButtonOpNode.h"
00062 #include "TimeGateNode.h"
00063 #include "EllipsoidTransformNode.h"
00064 #include "GKTransformNode.h"
00065 #include "RangeFilterNode.h"
00066 #include "PositionFilterNode.h"
00067 #include "EventUtilityNode.h"
00068
00069 #include <math.h>
00070 #include <float.h>
00071 #include <stdio.h>
00072 #include <iostream>
00073
00074 #include <algorithm>
00075
00076
00077
00078 #include <ace/Log_Msg.h>
00079 #include "../tool/OT_ACE_Log.h"
00080
00081
00082
00083 namespace ot {
00084
00085 CommonNodeFactory::CommonNodeFactory()
00086 {
00087 nodePorts.push_back("MergeAttribute");
00088 nodePorts.push_back("MergeTime");
00089 nodePorts.push_back("MergeTrigger");
00090 nodePorts.push_back("MergeDefault");
00091 nodePorts.push_back("TransformBase");
00092 nodePorts.push_back("Select");
00093 nodePorts.push_back("Arg1");
00094 nodePorts.push_back("Arg2");
00095 nodePorts.push_back("Gate");
00096 }
00097
00098 CommonNodeFactory::~CommonNodeFactory()
00099 {
00100 nodePorts.clear();
00101 }
00102
00103 int CommonNodeFactory::parseVector(const std::string & line, float * val )
00104 {
00105 float help[3];
00106 int num;
00107 num = sscanf( line.c_str()," %f %f %f", &help[0], &help[1], &help[2] );
00108 if( num != 3 )
00109 {
00110 return 1;
00111 }
00112 val[0] = help[0];
00113 val[1] = help[1];
00114 val[2] = help[2];
00115 return 0;
00116 }
00117
00118 int CommonNodeFactory::parseRotation(const std::string & line,const std::string & type, float * val )
00119 {
00120 int num;
00121 float matrix[3][3];
00122
00123 if( type.compare("quaternion") == 0 )
00124 {
00125 float help[4];
00126 num = sscanf(line.c_str()," %f %f %f %f",&help[0], &help[1], &help[2], &help[3]);
00127 if( num != 4 )
00128 {
00129 return 1;
00130 }
00131 val[0] = help[0];
00132 val[1] = help[1];
00133 val[2] = help[2];
00134 val[3] = help[3];
00135 MathUtils::normalizeQuaternion( val );
00136 }
00137 else if( type.compare("matrix") == 0 )
00138 {
00139 num = sscanf(line.c_str()," %f %f %f %f %f %f %f %f %f",
00140 &matrix[0][0], &matrix[0][1], &matrix[0][2],
00141 &matrix[1][0], &matrix[1][1], &matrix[1][2],
00142 &matrix[2][0], &matrix[2][1], &matrix[2][2] );
00143 if( num != 9 )
00144 {
00145 return 1;
00146 }
00147 else
00148 {
00149 float det = MathUtils::determinant( matrix );
00150 if( det < 0.9 || det > 1.1 )
00151 {
00152 printf("Matrix is not a pure rotation matrix !\n");
00153 return 1;
00154 }
00155 MathUtils::matrixToQuaternion( matrix, val );
00156 }
00157 }
00158 else if( type.compare("euler") == 0 )
00159 {
00160 float roll, pitch, yaw;
00161 num = sscanf(line.c_str()," %f %f %f", &roll, &pitch, &yaw );
00162 if( num != 3 )
00163 {
00164 return 1;
00165 }
00166 else
00167 {
00168 MathUtils::eulerToQuaternion( roll, pitch, yaw, val );
00169 }
00170 }
00171 else if( type.compare("axisangle") == 0 )
00172 {
00173 float axisa[4];
00174 num = sscanf(line.c_str()," %f %f %f %f", &axisa[0], &axisa[1], &axisa[2], &axisa[3] );
00175 if( num != 4 )
00176 {
00177 return 1;
00178 }
00179 else
00180 {
00181 MathUtils::axisAngleToQuaternion( axisa, val );
00182 }
00183 }
00184 else
00185 {
00186 return 1;
00187 }
00188 return 0;
00189 }
00190
00191
00192
00193 Node * CommonNodeFactory::createNode( const std::string& name, StringTable& attributes)
00194 {
00195 float translation[3] = {0,0,0}, scale[3] = {1,1,1}, rot[4]={0,0,0,1};
00196 Node * result = NULL;
00197 if( name.compare("EventPositionTransform") == 0 ||
00198 name.compare("QueuePositionTransform") == 0 ||
00199 name.compare("TimePositionTransform") == 0 )
00200 {
00201 if( parseVector(attributes.get("translation"), translation ) != 0 )
00202 {
00203 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error parsing translation !\n")));
00204 }
00205 if( parseVector(attributes.get("scale"), scale ) != 0 )
00206 {
00207 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error parsing scale !\n")));
00208 }
00209 if( parseRotation(attributes.get("rotation"),
00210 attributes.get("rotationtype"), rot ) != 0 )
00211 {
00212 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error parsing rotation !\n")));
00213 }
00214 result = new StaticTransformation( translation, scale, rot, true, false );
00215 }
00216 else if( name.compare("EventOrientationTransform") == 0 ||
00217 name.compare("QueueOrientationTransform") == 0 ||
00218 name.compare("TimeOrientationTransform") == 0 )
00219 {
00220 if( parseRotation(attributes.get("rotation"),
00221 attributes.get("rotationtype"), rot ) != 0 )
00222 {
00223 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error parsing rotation !\n")));
00224 }
00225 result = new StaticTransformation( translation, scale, rot, false, true );
00226 }
00227 else if( name.compare("EventTransform") == 0 ||
00228 name.compare("QueueTransform") == 0 ||
00229 name.compare("TimeTransform") == 0 )
00230 {
00231 if( parseVector(attributes.get("translation"), translation ) != 0 )
00232 {
00233 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error parsing translation !\n")));
00234 }
00235 if( parseVector(attributes.get("scale"), scale ) != 0 )
00236 {
00237 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error parsing scale !\n")));
00238 }
00239 if( parseRotation(attributes.get("rotation"),
00240 attributes.get("rotationtype"), rot ) != 0 )
00241 {
00242 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error parsing rotation !\n")));
00243 }
00244 result = new StaticTransformation( translation, scale, rot, true, true );
00245 }
00246 else if( name.compare("EventVirtualTransform") == 0 ||
00247 name.compare("QueueVirtualTransform") == 0 ||
00248 name.compare("TimeVirtualTransform") == 0 )
00249 {
00250 if( parseVector(attributes.get("translation"), translation ) != 0 )
00251 {
00252 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error parsing translation !\n")));
00253 }
00254 if( parseRotation(attributes.get("rotation"),
00255 attributes.get("rotationtype"), rot ) != 0 )
00256 {
00257 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error parsing rotation !\n")));
00258 }
00259 result = new VirtualTransformation( translation, scale, rot, 1, 1 );
00260 }
00261 else if( name.compare("EventVirtualPositionTransform") == 0 ||
00262 name.compare("QueueVirtualPositionTransform") == 0 ||
00263 name.compare("TimeVirtualPositionTransform") == 0 )
00264 {
00265 if( parseVector(attributes.get("translation"), translation ) != 0 )
00266 {
00267 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error parsing translation !\n")));
00268 }
00269 result = new VirtualTransformation( translation, scale, rot, 1, 0 );
00270 }
00271 else if( name.compare("EventVirtualOrientationTransform") == 0 ||
00272 name.compare("QueueVirtualOrientationTransform") == 0 ||
00273 name.compare("TimeVirtualOrientationTransform") == 0 )
00274 {
00275 if( parseRotation(attributes.get("rotation"),
00276 attributes.get("rotationtype"), rot ) != 0 )
00277 {
00278 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error parsing rotation !\n")));
00279 }
00280 result = new VirtualTransformation( translation, scale, rot, 0, 1 );
00281 }
00282 #ifndef OT_NO_EVENTUTITLY_SUPPORT
00283 else if( name.compare("EventQueue") == 0 )
00284 {
00285 int length;
00286 if( attributes.get("length", &length) != 1 )
00287 {
00288 length = 1;
00289 }
00290 result = new EventQueueNode( length );
00291 }
00292 #endif
00293 #ifndef OT_NO_MERGENODE_SUPPORT
00294 else if( name.compare("Merge") == 0 )
00295 {
00296 float agingFactor;
00297 MergeNode::CONF_CALCULATION confCalculation;
00298 attributes.get("agingFactor", &agingFactor);
00299 if (attributes.get("confidenceCalculation").compare("max") == 0)
00300 confCalculation = MergeNode::MAX;
00301 else if (attributes.get("confidenceCalculation").compare("multiply") == 0)
00302 confCalculation = MergeNode::MULTIPLY;
00303 else
00304 confCalculation = MergeNode::MIN;
00305 result = new MergeNode(agingFactor, confCalculation);
00306 }
00307 else if( name.compare("TriangulateOrientation") == 0)
00308 {
00309 result = new TriangulateOrientationNode();
00310 }
00311
00312 #endif
00313 #ifndef OT_NO_SELECTION_SUPPORT
00314 else if( name.compare("Selection") == 0 )
00315 {
00316 double timeOut = 100;
00317 attributes.get("timeout", &timeOut, 1 );
00318 result = new SelectionNode(timeOut);
00319 }
00320 #endif
00321 else if( name.compare("EventDynamicTransform") == 0 ||
00322 name.compare("QueueDynamicTransform") == 0 ||
00323 name.compare("TimeDynamicTransform") == 0 )
00324 {
00325 int baseEvent;
00326 if( attributes.get("baseevent").compare("true") == 0 )
00327 baseEvent = 1;
00328 else
00329 baseEvent = 0;
00330 result = new DynamicTransformation( baseEvent, true, true );
00331 }
00332 else if( name.compare("EventDynamicPositionTransform") == 0 ||
00333 name.compare("QueueDynamicPositionTransform") == 0 ||
00334 name.compare("TimeDynamicPositionTransform") == 0 )
00335 {
00336 int baseEvent;
00337 if( attributes.get("baseevent").compare("true") == 0 )
00338 baseEvent = 1;
00339 else
00340 baseEvent = 0;
00341 result = new DynamicTransformation( baseEvent, true, false );
00342 }
00343 else if( name.compare("EventDynamicOrientationTransform") == 0 ||
00344 name.compare("QueueDynamicOrientationTransform") == 0 ||
00345 name.compare("TimeDynamicOrientationTransform") == 0 )
00346 {
00347 int baseEvent;
00348 if( attributes.get("baseevent").compare("true") == 0 )
00349 baseEvent = 1;
00350 else
00351 baseEvent = 0;
00352 result = new DynamicTransformation( baseEvent, false, true );
00353 }
00354 else if( name.compare("EventInvertTransform") == 0 ||
00355 name.compare("QueueInvertTransform") == 0 ||
00356 name.compare("TimeInvertTransform") == 0 )
00357 {
00358 result = new InvertTransformation();
00359 }
00360 else if( name.compare("EventMatrixTransform") == 0 ||
00361 name.compare("QueueMatrixTransform") == 0 ||
00362 name.compare("TimeMatrixTransform") == 0 )
00363 {
00364 float data[12];
00365 attributes.get( "matrix", data, 12 );
00366 result = new MatrixTransformation( data );
00367 }
00368 #ifndef OT_NO_CONFIDENCE_SUPPORT
00369 else if( name.compare("ConfidenceFilter") == 0 )
00370 {
00371 float treshold= 0.5;
00372 ConfidenceFilterNode::types type = ConfidenceFilterNode::HIGH;
00373 attributes.get("treshhold", &treshold, 1 );
00374 if( attributes.get("type").compare("high") == 0 )
00375 type = ConfidenceFilterNode::HIGH;
00376 else if( attributes.get("type").compare("low") == 0 )
00377 type = ConfidenceFilterNode::LOW;
00378 result = new ConfidenceFilterNode( treshold, type );
00379 }
00380 #endif
00381 else if( name.compare("Filter") == 0 )
00382 {
00383 std::vector<float> weights;
00384 const char * data = attributes.get("weight").c_str();
00385 char * end = (char *) data;
00386 weights.push_back((float) strtod( data, &end ));
00387 while( end != data ){
00388 data = end;
00389 weights.push_back((float) strtod( data, &end ));
00390 }
00391 weights.pop_back();
00392 FilterNode::Type type = FilterNode::ALL;
00393 if( attributes.get("type").compare("position") == 0)
00394 type = FilterNode::POSITION;
00395 else if( attributes.get("type").compare("orientation") == 0 )
00396 type = FilterNode::ORIENTATION;
00397 ACE_DEBUG((LM_INFO, ACE_TEXT("ot:FilterNode with %d weights\n"), weights.size()));
00398 result = new FilterNode( weights, type );
00399 }
00400 #ifndef OT_NO_CONFIDENCE_SUPPORT
00401 else if( name.compare("ConfidenceSelect") == 0 )
00402 {
00403 double timeout = 100;
00404 ConfidenceSelectNode::types type = ConfidenceSelectNode::HIGH;
00405 attributes.get("timeout", &timeout, 1 );
00406 if( attributes.get("type").compare("high") == 0 )
00407 type = ConfidenceSelectNode::HIGH;
00408 else if( attributes.get("type").compare("low") == 0 )
00409 type = ConfidenceSelectNode::LOW;
00410 result = new ConfidenceSelectNode( timeout, type );
00411 }
00412 #endif
00413 #ifndef OT_NO_THRESHOLDFILTER_SUPPORT
00414 else if( name.compare("ThresholdFilter") == 0 )
00415 {
00416 float posmin = 0, posmax = FLT_MAX, rotmin = 0, rotmax = 3.141592654f;
00417 if( attributes.get("positionmax").compare("inf") == 0 )
00418 posmax = FLT_MAX;
00419 else
00420 attributes.get("positionmax", &posmax, 1 );
00421 attributes.get("positionmin", &posmin, 1 );
00422 attributes.get("rotationmin", &rotmin, 1 );
00423 attributes.get("rotationmax", &rotmax, 1 );
00424 if( posmin < 0 )
00425 posmin = 0;
00426 if( posmax < posmin )
00427 posmax = posmin;
00428 if( rotmin < 0 )
00429 rotmin = 0;
00430 if( rotmax < rotmin )
00431 rotmax = rotmin;
00432 if( rotmax > 3.141592654f )
00433 rotmax = 3.141592654f;
00434 result = new ThresholdFilterNode( posmin, posmax, rotmin, rotmax );
00435 }
00436 #endif
00437 #ifndef OT_NO_BUTTON_SUPPORT
00438 else if( name.compare("ButtonFilter") == 0 )
00439 {
00440 result = new ButtonFilterNode( attributes.get("buttonmask").data(), attributes.get("buttonmap").data(), attributes.get("invert").data(),
00441 attributes.get("validtrans").data() , attributes.get("radiobuttons").data(), attributes.get("setbuttononvalidtrans").data());
00442 }
00443 #endif
00444 #ifndef OT_NO_CONFIDENCE_SUPPORT
00445 else if( name.compare("ButtonOp") == 0 )
00446 {
00447 result = new ButtonOpNode( (attributes.get("op").compare("OR") == 0)?(ButtonOpNode::OR):(ButtonOpNode::AND));
00448 }
00449 #endif
00450 #ifndef OT_NO_TIMEGATE_SUPPORT
00451 else if( name.compare("TimeGate") == 0 )
00452 {
00453 double timeframe;
00454 attributes.get("timeframe", &timeframe );
00455 TimeGateNode::Mode mode = (attributes.get("mode").compare("pass") == 0)?
00456 (TimeGateNode::PASS):(TimeGateNode::BLOCK);
00457 result = new TimeGateNode( timeframe, mode );
00458 }
00459 #endif
00460 #ifndef OT_NO_ELLIPSOIDTRANSFORM_SUPPORT
00461 else if( name.compare("EventEllipsoidTransform") == 0 ||
00462 name.compare("QueueEllipsoidTransform") == 0 ||
00463 name.compare("TimeEllipsoidTransform") == 0 )
00464 {
00465 double a;
00466 double b;
00467 EllipsoidTransformNode::Mode mode;
00468 attributes.get("a", &a );
00469 if( attributes.containsKey("b"))
00470 {
00471 attributes.get("b", &b );
00472 }
00473 else
00474 b = a;
00475 if( attributes.get("mode").compare("toEllipsoid") == 0 )
00476 mode = EllipsoidTransformNode::toEllipsoid;
00477 else
00478 mode = EllipsoidTransformNode::toCartesian;
00479 result = new EllipsoidTransformNode( a, b, mode );
00480 }
00481 #endif
00482 #ifndef OT_NO_CONFIDENCE_SUPPORT
00483 else if( name.compare("EventGKTransform") == 0 ||
00484 name.compare("QueueGKTransform") == 0 ||
00485 name.compare("TimeGKTransform") == 0 )
00486 {
00487 double a = 0, b = 0, meridian = 0, alpha = 0, beta = 0, gamma = 0, delta = 0;
00488 GKTransformNode::Mode mode;
00489 attributes.get("a", &a );
00490 if( attributes.containsKey("b"))
00491 {
00492 attributes.get("b", &b );
00493 }
00494 else
00495 b = a;
00496 attributes.get("meridian", &meridian );
00497 attributes.get("alpha", &alpha );
00498 attributes.get("beta", &beta );
00499 attributes.get("gamma", &gamma );
00500 attributes.get("delta", &delta );
00501 if( attributes.get("mode").compare("to") == 0 )
00502 mode = GKTransformNode::to;
00503 else
00504 mode = GKTransformNode::from;
00505 result = new GKTransformNode( a, b, meridian, alpha, beta, gamma, delta, mode );
00506 }
00507 #endif
00508 #ifndef OT_NO_RANGEFILTER_SUPPORT
00509 else if( name.compare("RangeFilter") == 0 )
00510 {
00511 float min, max;
00512 if( attributes.get("min", &min) != 1 )
00513 min = 0;
00514 if( attributes.get("max", &max ) != 1 )
00515 max = (float)sqrt(FLT_MAX)-0.1f;
00516 result = new RangeFilterNode( min, max );
00517 }
00518 #endif
00519 #ifndef OT_NO_POSITIONFILTER_SUPPORT
00520 else if( name.compare("PositionFilterNode") == 0 )
00521 {
00522 float min[3], max[3];
00523 if( attributes.get("min", min, 3) != 3)
00524 { min[0] = -1; min[1] = -1; min[2] = -1;}
00525 if( attributes.get("max", max, 3) != 3 )
00526 { max[0] = 1; max[1] = 1; max[2] = 1;}
00527 result = new PositionFilterNode( min, max );
00528 }
00529 #endif
00530 #ifndef OT_NO_EVENTUTITLY_SUPPORT
00531 else if ( name.compare("EventUtilityNode") == 0 )
00532 {
00533 std::string rename = attributes.get("rename");
00534 std::string create = attributes.get("create");
00535 std::string discard = attributes.get("discard");
00536
00537 EventUtilityNode *resultNode = new EventUtilityNode();
00538
00539 if (rename.length() > 0)
00540 {
00541 std::string::size_type firstBlank = rename.find(' ');
00542 if (firstBlank == std::string::npos)
00543 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error parsing names for renaming in EventUtilityNode!\n")));
00544 else
00545 {
00546 std::string oldName = rename.substr(0, firstBlank);
00547 std::string newName = rename.substr(firstBlank + 1, std::string::npos);
00548 resultNode->setRename(oldName, newName);
00549 }
00550 }
00551 if (create.length() > 0)
00552 {
00553 std::string::size_type firstBlank = create.find(' ');
00554 std::string::size_type secondBlank = create.find(' ', firstBlank + 1);
00555 if (firstBlank == std::string::npos ||
00556 secondBlank == std::string::npos)
00557 {
00558 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error parsing serialized event for creation in EventUtilityNode!\n")));
00559 }
00560 else
00561 {
00562 std::string type = create.substr(0, firstBlank);
00563 std::string name = create.substr(firstBlank + 1, secondBlank - (firstBlank + 1));
00564 std::string value = create.substr(secondBlank + 1, std::string::npos);
00565 resultNode->setCreate(type, name, value);
00566 }
00567 }
00568 if (discard.length() > 0)
00569 resultNode->setDiscard(discard);
00570
00571 result = resultNode;
00572 }
00573 #endif //OT_NO_EVENTUTITLY_SUPPORT
00574
00575
00576 else if( std::find( nodePorts.begin(), nodePorts.end(), name ) != nodePorts.end())
00577 {
00578 LOG_ACE_INFO("ot:Build NodePort %s.\n", name.c_str());
00579 return new NodePort();
00580 }
00581 if( result != NULL )
00582 {
00583 LOG_ACE_INFO("ot:Build %s node.\n", name.c_str());
00584 }
00585 return result;
00586 }
00587
00588 }
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604