1 /* 2 Copyright (c) 2008-2009 NetAllied Systems GmbH 3 4 This file is part of COLLADASaxFrameworkLoader. 5 6 Licensed under the MIT Open Source License, 7 for details please see LICENSE file or the website 8 http://www.opensource.org/licenses/mit-license.php 9 */ 10 11 #include "COLLADASaxFWLStableHeaders.h" 12 #include "COLLADASaxFWLKinematicsSceneCreator.h" 13 #include "COLLADASaxFWLFileLoader.h" 14 15 #include "COLLADAFWKinematicsScene.h" 16 #include "COLLADAFWKinematicsModel.h" 17 #include "COLLADAFWTranslate.h" 18 #include "COLLADAFWRotate.h" 19 20 21 namespace COLLADASaxFWL 22 { 23 appendTransformations(COLLADAFW::TransformationPointerArray & target,TransformationList source,bool invert=false)24 void appendTransformations( COLLADAFW::TransformationPointerArray& target, TransformationList source, bool invert = false) 25 { 26 if ( invert ) 27 { 28 TransformationList::const_reverse_iterator sourceIt = source.rbegin(); 29 TransformationList::const_reverse_iterator listEnd = source.rend(); 30 for ( ; sourceIt != listEnd; ++sourceIt ) 31 { 32 COLLADAFW::Transformation* sourceTransformation = *sourceIt; 33 COLLADAFW::Transformation* invertedTransformation = 0; 34 if ( sourceTransformation->getTransformationType() == COLLADAFW::Transformation::TRANSLATE ) 35 { 36 COLLADAFW::Translate* sourceTranslate = (COLLADAFW::Translate*)sourceTransformation; 37 COLLADAFW::Translate* invertedTranslate = FW_NEW COLLADAFW::Translate(); 38 invertedTranslate->setTranslation( -sourceTranslate->getTranslation() ); 39 invertedTransformation = invertedTranslate; 40 } 41 else if ( sourceTransformation->getTransformationType() == COLLADAFW::Transformation::ROTATE ) 42 { 43 COLLADAFW::Rotate* sourceRotate = (COLLADAFW::Rotate*)sourceTransformation; 44 COLLADAFW::Rotate* invertedRotate = FW_NEW COLLADAFW::Rotate(); 45 invertedRotate->setRotationAxis( sourceRotate->getRotationAxis() ); 46 invertedRotate->setRotationAngle( -sourceRotate->getRotationAngle() ); 47 invertedTransformation = invertedRotate; 48 } 49 COLLADABU_ASSERT( invertedTransformation ); 50 target.append( invertedTransformation ); 51 } 52 } 53 else 54 { 55 TransformationList::const_iterator sourceIt = source.begin(); 56 for ( ; sourceIt != source.end(); ++sourceIt ) 57 { 58 target.append( (*sourceIt)->clone() ); 59 } 60 } 61 } 62 63 //------------------------------ KinematicsSceneCreator(DocumentProcessor * documentProcessor)64 KinematicsSceneCreator::KinematicsSceneCreator( DocumentProcessor* documentProcessor ) 65 : mKinematicsScene(0) 66 , mDocumentProcessor(documentProcessor) 67 , mKinematicsIntermediateData( documentProcessor->getKinematicsIntermediateData()) 68 , mLargestLinkNumber(0) 69 , mLargestJointIndex(0) 70 { 71 72 } 73 74 //------------------------------ ~KinematicsSceneCreator()75 KinematicsSceneCreator::~KinematicsSceneCreator() 76 { 77 } 78 79 //------------------------------ createAndGetKinematicsScene()80 COLLADAFW::KinematicsScene* KinematicsSceneCreator::createAndGetKinematicsScene() 81 { 82 mKinematicsScene = FW_NEW COLLADAFW::KinematicsScene(); 83 84 //Kinematics models 85 COLLADAFW::KinematicsModelArray& fwKinematicsModels = mKinematicsScene->getKinematicsModels(); 86 87 const KinematicsIntermediateData::KinematicsModelMap& kinematicsModels = mKinematicsIntermediateData.getKinematicsModels(); 88 KinematicsIntermediateData::KinematicsModelMap::const_iterator modelIt = kinematicsModels.begin(); 89 for ( ; modelIt != kinematicsModels.end(); ++modelIt ) 90 { 91 KinematicsModel* kinematicsModel = modelIt->second; 92 93 COLLADAFW::KinematicsModel* fwKinematicsModel = createFWKinematicsModel(kinematicsModel); 94 mKinematicsModelFWKinematicsModelMap.insert(std::make_pair(kinematicsModel, fwKinematicsModel)); 95 fwKinematicsModels.append( fwKinematicsModel ); 96 mLargestJointIndex = 0; 97 mJointIndexMap.clear(); 98 } 99 100 //Kinematics controllers 101 COLLADAFW::KinematicsControllerArray& fwKinematicsControllers = mKinematicsScene->getKinematicsControllers(); 102 103 const KinematicsIntermediateData::KinematicsControllerMap& kinematicsControllers = mKinematicsIntermediateData.getKinematicsControllers(); 104 KinematicsIntermediateData::KinematicsControllerMap::const_iterator controllerIt = kinematicsControllers.begin(); 105 for ( ; controllerIt != kinematicsControllers.end(); ++controllerIt ) 106 { 107 KinematicsController* kinematicsController = controllerIt->second; 108 109 COLLADAFW::KinematicsController* fwKinematicsController = createFWKinematicsController(kinematicsController); 110 fwKinematicsControllers.append( fwKinematicsController ); 111 } 112 113 114 //Instance kinematics scenes 115 COLLADAFW::InstanceKinematicsSceneArray& fwInstanceKinematicsScenes = mKinematicsScene->getInstanceKinematicsScenes(); 116 117 const KinematicsIntermediateData::KinematicsInstanceKinematicsScenes& instanceKinematicsScenes = mKinematicsIntermediateData.getInstanceKinematicsScenes(); 118 KinematicsIntermediateData::KinematicsInstanceKinematicsScenes::const_iterator instanceKinematicsSceneIt = instanceKinematicsScenes.begin(); 119 for ( ; instanceKinematicsSceneIt != instanceKinematicsScenes.end(); ++instanceKinematicsSceneIt ) 120 { 121 KinematicsInstanceKinematicsScene* instanceKinematicsScene= *instanceKinematicsSceneIt; 122 123 COLLADAFW::InstanceKinematicsScene* fwInstanceKinematicsScene = createFWInstanceKinematicsScene(instanceKinematicsScene); 124 fwInstanceKinematicsScenes.append( fwInstanceKinematicsScene ); 125 } 126 127 return mKinematicsScene; 128 } 129 130 //------------------------------ createFWKinematicsModel(KinematicsModel * kinematicsModel)131 COLLADAFW::KinematicsModel* KinematicsSceneCreator::createFWKinematicsModel(KinematicsModel* kinematicsModel) 132 { 133 COLLADAFW::UniqueId uniqueId = mDocumentProcessor->createUniqueIdFromUrl( kinematicsModel->getUrl(), COLLADAFW::KinematicsModel::ID(), true); 134 COLLADAFW::KinematicsModel* fwKinematicsModel = FW_NEW COLLADAFW::KinematicsModel(uniqueId); 135 136 COLLADAFW::SizeTValuesArray& fwBaseLinks = fwKinematicsModel->getBaseLinks(); 137 138 const KinematicLinkList& baseLinks = kinematicsModel->getBaseLinks(); 139 140 KinematicLinkList::const_iterator it = baseLinks.begin(); 141 142 for ( ; it != baseLinks.end(); ++it ) 143 { 144 KinematicLink* link = *it; 145 fwBaseLinks.append( mLargestLinkNumber ); 146 mLinkNumberStack.push(mLargestLinkNumber++); 147 createJointLinkConnections( link, fwKinematicsModel ); 148 } 149 150 return fwKinematicsModel; 151 } 152 153 //------------------------------ createJointLinkConnections(const KinematicLink * link,COLLADAFW::KinematicsModel * fwKinematicsModel)154 void KinematicsSceneCreator::createJointLinkConnections(const KinematicLink* link, COLLADAFW::KinematicsModel* fwKinematicsModel) 155 { 156 COLLADAFW::KinematicsModel::LinkJointConnections& linkJointConnections = fwKinematicsModel->getLinkJointConnections(); 157 158 //TODO UNUSED const TransformationList& linkTransformations = link->getTransformations(); 159 160 const KinematicAttachmentList& attachments = link->getAttachments(); 161 KinematicAttachmentList::const_iterator it = attachments.begin(); 162 for ( ; it != attachments.end(); ++it ) 163 { 164 KinematicAttachment* attachment = *it; 165 166 const SidAddress& jointAddress = attachment->getJoint(); 167 const SidTreeNode* jointTreeNode = mDocumentProcessor->resolveSid( jointAddress ); 168 if ( !jointTreeNode ) 169 { 170 //TODO: handle error 171 continue; 172 } 173 174 COLLADAFW::Joint* joint = 0; 175 // Used if the joint is an instance joint in collada 176 const COLLADAFW::UniqueId* jointUniqueId = 0; 177 178 if ( jointTreeNode->getTargetType() == SidTreeNode::TARGETTYPECLASS_INTERMEDIATETARGETABLE ) 179 { 180 //this must be a KinematicInstance 181 const KinematicInstance* instanceJoint = intermediateTargetableSafeCast<KinematicInstance>(jointTreeNode->getIntermediateTargetableTarget()); 182 COLLADABU_ASSERT(instanceJoint); 183 SidAddress referencedJointAddress( instanceJoint->getUrl() ); 184 const SidTreeNode* referencedJointTreeNode = mDocumentProcessor->resolveSid( referencedJointAddress ); 185 186 COLLADABU_ASSERT( referencedJointTreeNode->getTargetType() == SidTreeNode::TARGETTYPECLASS_OBJECT ); 187 jointUniqueId = &instanceJoint->getReplacingObjectUniqueId(); 188 joint = COLLADAFW::objectSafeCast<COLLADAFW::Joint>(referencedJointTreeNode->getObjectTarget()); 189 } 190 else if ( jointTreeNode->getTargetType() == SidTreeNode::TARGETTYPECLASS_OBJECT ) 191 { 192 joint = COLLADAFW::objectSafeCast<COLLADAFW::Joint>(jointTreeNode->getObjectTarget()); 193 } 194 195 if ( !joint ) 196 { 197 //TODO: handle error 198 COLLADABU_ASSERT(joint); 199 continue; 200 } 201 202 size_t jointIndex = 0; 203 JointIndexMap::const_iterator jointIndexIt = mJointIndexMap.find(joint); 204 if ( jointIndexIt == mJointIndexMap.end() ) 205 { 206 // this joint is not in the kinematics model. Add it here. 207 jointIndex = mLargestJointIndex; 208 mJointIndexMap[joint] = mLargestJointIndex++; 209 COLLADAFW::JointPointerArray& fwJoints = fwKinematicsModel->getJoints(); 210 // If the joint is in the kinematics model in COLLADA we clone it completely, if it 211 // is instantiated we need to take the unique id from the instance 212 COLLADAFW::Joint* clonedJoint = jointUniqueId ? joint->clone(*jointUniqueId) : joint->clone(); 213 fwJoints.append( clonedJoint ); 214 215 const COLLADAFW::JointPrimitivePointerArray& jointPrimitives = joint->getJointPrimitives(); 216 const COLLADAFW::JointPrimitivePointerArray& clonedJointPrimitives = clonedJoint->getJointPrimitives(); 217 COLLADABU_ASSERT(jointPrimitives.getCount() == clonedJointPrimitives.getCount() ); 218 for ( size_t j = 0; j < jointPrimitives.getCount(); ++j) 219 { 220 mOriginalClonedJointPrimitiveMap.insert(std::make_pair(jointPrimitives[j], clonedJointPrimitives[j] )); 221 } 222 223 } 224 else 225 { 226 // this joint is in the kinematics model. take it. 227 jointIndex = jointIndexIt->second; 228 } 229 230 COLLADAFW::KinematicsModel::LinkJointConnection *linkJointConnection = FW_NEW COLLADAFW::KinematicsModel::LinkJointConnection( mLinkNumberStack.top(), jointIndex ); 231 232 COLLADAFW::TransformationPointerArray& linkJointConnectionTransformations =linkJointConnection->getTransformations(); 233 linkJointConnectionTransformations.reallocMemory( attachment->getTransformations().size()); 234 appendTransformations(linkJointConnectionTransformations, attachment->getTransformations()); 235 linkJointConnections.append(linkJointConnection); 236 createJointLinkConnections(attachment, jointIndex, fwKinematicsModel); 237 } 238 239 } 240 241 //------------------------------ createJointLinkConnections(const KinematicAttachment * attachment,size_t jointIndex,COLLADAFW::KinematicsModel * fwKinematicsModel)242 void KinematicsSceneCreator::createJointLinkConnections(const KinematicAttachment* attachment, size_t jointIndex, COLLADAFW::KinematicsModel* fwKinematicsModel) 243 { 244 COLLADAFW::KinematicsModel::LinkJointConnections& linkJointConnections = fwKinematicsModel->getLinkJointConnections(); 245 246 // @TODO UNUSED const TransformationList& linkTransformations = attachment->getTransformations(); 247 248 mLinkNumberStack.push(mLargestLinkNumber++); 249 250 const KinematicLink& link = attachment->getLink(); 251 252 COLLADAFW::KinematicsModel::LinkJointConnection *linkJointConnection = FW_NEW COLLADAFW::KinematicsModel::LinkJointConnection( mLinkNumberStack.top(), jointIndex ); 253 254 COLLADAFW::TransformationPointerArray& linkJointConnectionTransformations =linkJointConnection->getTransformations(); 255 linkJointConnectionTransformations.reallocMemory( link.getTransformations().size() ); 256 appendTransformations(linkJointConnectionTransformations, link.getTransformations(), true); 257 linkJointConnections.append(linkJointConnection); 258 createJointLinkConnections(&link, fwKinematicsModel); 259 mLinkNumberStack.pop(); 260 } 261 262 //------------------------------ createFWKinematicsController(KinematicsController * kinematicsController)263 COLLADAFW::KinematicsController* KinematicsSceneCreator::createFWKinematicsController(KinematicsController* kinematicsController) 264 { 265 COLLADAFW::UniqueId uniqueId = mDocumentProcessor->createUniqueIdFromUrl( kinematicsController->getUri(), COLLADAFW::KinematicsController::ID(), true); 266 COLLADAFW::KinematicsController* fwKinematicsController = FW_NEW COLLADAFW::KinematicsController(uniqueId); 267 268 // get instance kinematics models 269 const KinematicsInstanceKinematicsModels& instanceKinematicsModels = kinematicsController->getKinematicsInstanceKinematicsModels(); 270 KinematicsInstanceKinematicsModels::const_iterator instanceIt = instanceKinematicsModels.begin(); 271 272 typedef std::vector<COLLADAFW::UniqueId> UniqueIdVector; 273 UniqueIdVector instanceKinematicsModelVector; 274 275 for ( ; instanceIt != instanceKinematicsModels.end(); ++instanceIt ) 276 { 277 const KinematicsInstanceKinematicsModel& instanceKinematicsModel = *instanceIt; 278 279 COLLADAFW::UniqueId kinematicsModelUniqueId = processInstanceKinematicsModel(instanceKinematicsModel); 280 if ( kinematicsModelUniqueId.isValid() ) 281 { 282 instanceKinematicsModelVector.push_back(kinematicsModelUniqueId); 283 } 284 } 285 286 // fill kinematicsModelUniqueIds 287 COLLADAFW::UniqueIdArray& kinematicsModelUniqueIds = fwKinematicsController->getKinematicsModelUniqueIds(); 288 size_t instanceKinematicsModelsCount = instanceKinematicsModelVector.size(); 289 kinematicsModelUniqueIds.allocMemory( instanceKinematicsModelsCount ); 290 for ( size_t i = 0; i < instanceKinematicsModelsCount; ++i) 291 { 292 kinematicsModelUniqueIds[i] = instanceKinematicsModelVector[i]; 293 } 294 kinematicsModelUniqueIds.setCount( instanceKinematicsModelsCount ); 295 296 // create axis infos 297 COLLADAFW::AxisInfoArray& fwAxisInfos = fwKinematicsController->getAxisInfos(); 298 299 const AxisInfoList& axisInfos = kinematicsController->getAxisInfos(); 300 301 AxisInfoList::const_iterator axisInfoIt = axisInfos.begin(); 302 303 for ( ; axisInfoIt != axisInfos.end(); ++axisInfoIt ) 304 { 305 const AxisInfo& axisInfo = *axisInfoIt; 306 307 bool success = true; 308 COLLADAFW::AxisInfo fwAxisInfo = createFWAxisInfo(axisInfo, success); 309 if ( success ) 310 { 311 fwAxisInfos.append(fwAxisInfo); 312 } 313 314 // fwAxisInfos.append( mLargestLinkNumber ); 315 // mLinkNumberStack.push(mLargestLinkNumber++); 316 // createJointLinkConnections( axisInfo, fwKinematicsController ); 317 } 318 319 return fwKinematicsController; 320 } 321 322 //------------------------------ createFWAxisInfo(const AxisInfo & axisInfo,bool & success)323 COLLADAFW::AxisInfo KinematicsSceneCreator::createFWAxisInfo( const AxisInfo& axisInfo, bool& success ) 324 { 325 const SidAddress& jointPrimitiveAddress = axisInfo.getJointPrimitiveRefAddress(); 326 const SidTreeNode* jointPrimitiveSidTreeNode = mDocumentProcessor->resolveSid( jointPrimitiveAddress ); 327 if ( !jointPrimitiveSidTreeNode ) 328 { 329 // TODO report error joint primitive could not be resolved 330 success = false; 331 return COLLADAFW::AxisInfo(); 332 } 333 334 if ( jointPrimitiveSidTreeNode->getTargetType() != SidTreeNode::TARGETTYPECLASS_OBJECT ) 335 { 336 // TODO report error joint primitive could not be resolved 337 success = false; 338 return COLLADAFW::AxisInfo(); 339 } 340 341 COLLADAFW::Object* object = jointPrimitiveSidTreeNode->getObjectTarget(); 342 343 if ( object->getClassId() != COLLADAFW::JointPrimitive::ID() ) 344 { 345 // TODO report error joint primitive could not be resolved 346 success = false; 347 return COLLADAFW::AxisInfo(); 348 } 349 350 COLLADAFW::JointPrimitive* jointPrimitive= (COLLADAFW::JointPrimitive*)object; 351 352 success = true; 353 COLLADAFW::AxisInfo fwAxisInfo; 354 fwAxisInfo.setIsActive( axisInfo.getIsActive() ); 355 fwAxisInfo.setIsLocked( axisInfo.getIsLocked() ); 356 fwAxisInfo.setIndex( axisInfo.getIndex() ); 357 358 JointPrimitiveJointPrimitiveMap::const_iterator it = mOriginalClonedJointPrimitiveMap.find(jointPrimitive); 359 if ( it == mOriginalClonedJointPrimitiveMap.end() ) 360 { 361 fwAxisInfo.setJointPrimitive( 0 ); 362 363 } 364 else 365 { 366 fwAxisInfo.setJointPrimitive( it->second ); 367 } 368 return fwAxisInfo; 369 } 370 371 //------------------------------ processInstanceKinematicsModel(const KinematicsInstanceKinematicsModel & instanceKinematicsModel)372 COLLADAFW::UniqueId KinematicsSceneCreator::processInstanceKinematicsModel(const KinematicsInstanceKinematicsModel& instanceKinematicsModel) 373 { 374 COLLADAFW::UniqueId kinematicsModelUniqueId = mDocumentProcessor->getUniqueIdByUrl( instanceKinematicsModel.getUrl(), true ); 375 if ( !kinematicsModelUniqueId.isValid() ) 376 { 377 return COLLADAFW::UniqueId::INVALID; 378 } 379 380 return kinematicsModelUniqueId; 381 } 382 383 //------------------------------ createFWInstanceKinematicsScene(KinematicsInstanceKinematicsScene * instanceKinematicsScene)384 COLLADAFW::InstanceKinematicsScene* KinematicsSceneCreator::createFWInstanceKinematicsScene( KinematicsInstanceKinematicsScene* instanceKinematicsScene ) 385 { 386 //COLLADAFW::UniqueId uniqueId = mDocumentProcessor->createUniqueIdFromUrl( instanceKinematicsScene->getUrl(), COLLADAFW::KinematicsController::ID(), true); 387 COLLADAFW::UniqueId uniqueId = mDocumentProcessor->createUniqueId( COLLADAFW::InstanceKinematicsScene::ID()); 388 COLLADAFW::InstanceKinematicsScene* fwInstanceKinematicsScene = FW_NEW COLLADAFW::InstanceKinematicsScene(uniqueId, COLLADAFW::UniqueId::INVALID); 389 390 const COLLADABU::URI& instanceKinematicsUrl = instanceKinematicsScene->getUrl(); 391 KinematicsScene* kinematicsScene = mDocumentProcessor->getKinematicsSceneByUri(instanceKinematicsUrl); 392 if ( !kinematicsScene ) 393 { 394 String msg = "Kinematics scene \"" + instanceKinematicsUrl.getURIString() + "\" could not be found."; 395 396 mDocumentProcessor->handleFWLError(SaxFWLError::ERROR_UNRESOLVED_REFERENCE, msg); 397 return 0; 398 } 399 400 const KinematicsBindJointAxes& kinematicsBindJointAxes = instanceKinematicsScene->getBindJointAxes(); 401 402 KinematicsBindJointAxes::const_iterator kinematicsBindJointAxisIt = kinematicsBindJointAxes.begin(); 403 for ( ; kinematicsBindJointAxisIt != kinematicsBindJointAxes.end(); ++kinematicsBindJointAxisIt ) 404 { 405 KinematicsBindJointAxis* kinematicsBindJointAxis= *kinematicsBindJointAxisIt; 406 407 408 // find the unique id of the node a link is bound to 409 const String& nodeId = kinematicsBindJointAxis->getTarget().getId(); 410 COLLADABU::URI nodeUri = instanceKinematicsScene->getUrl(); 411 nodeUri.setFragment(nodeId); 412 const COLLADAFW::UniqueId nodeUniqueId = mDocumentProcessor->getUniqueIdByUrl(nodeUri, true); 413 if ( !nodeUniqueId.isValid() ) 414 { 415 String msg = "Node with id \"" + nodeId + "\" referenced in <bind_joint_axis> in <bind_kinematics_model> could not be found."; 416 417 mDocumentProcessor->handleFWLError(SaxFWLError::ERROR_UNRESOLVED_REFERENCE, msg); 418 continue; 419 } 420 421 size_t linkNumber = 0; 422 COLLADAFW::KinematicsModel* kinematicsModel = 0; 423 if ( !resolveLink(kinematicsScene, kinematicsBindJointAxis, &linkNumber, &kinematicsModel) ) 424 { 425 String msg; 426 switch ( kinematicsBindJointAxis->getAxis().getValueType() ) 427 { 428 case KinematicsAxis::VALUETYPE_PARAM: 429 msg = "Axis with sid \"" + *kinematicsBindJointAxis->getAxis().getParamValue() + "\" could not be resolved."; 430 break; 431 case KinematicsAxis::VALUETYPE_SIDREF: 432 msg = "Axis with sid address \"" + kinematicsBindJointAxis->getAxis().getSidrefValue()->getSidAddressString() + "\" could not be resolved."; 433 break; 434 case KinematicsAxis::VALUETYPE_UNKNOWN: 435 break; 436 } 437 //todo: handle error smarter 438 // mDocumentProcessor->handleFWLError(SaxFWLError::ERROR_UNRESOLVED_REFERENCE, msg); 439 continue; 440 } 441 442 443 // find the kin model id 444 const COLLADAFW::KinematicsModelArray& kinematicsModels = mKinematicsScene->getKinematicsModels(); 445 size_t kinModelId = std::numeric_limits<size_t>::max(); 446 for ( size_t i = 0; i < kinematicsModels.getCount(); ++i) 447 { 448 if ( kinematicsModels[i] == kinematicsModel ) 449 { 450 kinModelId = i; 451 break; 452 } 453 } 454 455 COLLADAFW::InstanceKinematicsScene::NodeLinkBinding nodeLinkBinding = {nodeUniqueId, kinModelId, linkNumber}; 456 mNodeLinkBindingSet.insert(nodeLinkBinding); 457 458 } 459 460 COLLADAFW::InstanceKinematicsScene::NodeLinkBindingArray& nodeLinkBindings = fwInstanceKinematicsScene->getNodeLinkBindings(); 461 size_t nodeLinkBindingCount = mNodeLinkBindingSet.size(); 462 nodeLinkBindings.allocMemory(nodeLinkBindingCount); 463 nodeLinkBindings.setCount( nodeLinkBindingCount ); 464 NodeLinkBindingSet::const_iterator it = mNodeLinkBindingSet.begin(); 465 for ( size_t i = 0; it != mNodeLinkBindingSet.end(); ++it, ++i) 466 { 467 nodeLinkBindings[i] = *it; 468 } 469 470 mNodeLinkBindingSet.clear(); 471 return fwInstanceKinematicsScene; 472 } 473 474 //------------------------------ resolveLink(KinematicsScene * kinematicsScene,KinematicsBindJointAxis * kinematicsBindJointAxis,size_t * linkNumber,COLLADAFW::KinematicsModel ** kinModel)475 bool KinematicsSceneCreator::resolveLink( KinematicsScene* kinematicsScene, 476 KinematicsBindJointAxis* kinematicsBindJointAxis, 477 size_t* linkNumber, 478 COLLADAFW::KinematicsModel** kinModel ) 479 { 480 const KinematicsInstanceKinematicsModels& instanceKinematicsModels = kinematicsScene->getKinematicsInstanceKinematicsModels(); 481 KinematicsInstanceKinematicsModels::const_iterator it = instanceKinematicsModels.begin(); 482 483 const KinematicsAxis& axis = kinematicsBindJointAxis->getAxis(); 484 KinematicsAxis::ValueType axisValueType = axis.getValueType(); 485 if ( axisValueType != KinematicsAxis::VALUETYPE_PARAM ) 486 { 487 return false; 488 } 489 const String& param = *axis.getParamValue(); 490 491 for (; it != instanceKinematicsModels.end(); ++it) 492 { 493 const KinematicsInstanceKinematicsModel& instanceKinematicsModel = *it; 494 495 const COLLADABU::URI& kinematicsModelUrl = instanceKinematicsModel.getUrl(); 496 497 KinematicsModel* kinematicsModel = mDocumentProcessor->getKinematicsModelByUri(kinematicsModelUrl); 498 499 if ( !kinematicsModel ) 500 { 501 String msg = "Kinematics Model \"" + kinematicsModelUrl.getURIString() + "\" could not be found."; 502 503 mDocumentProcessor->handleFWLError(SaxFWLError::ERROR_UNRESOLVED_REFERENCE, msg); 504 continue; 505 } 506 507 KinematicsNewParam* newParam = instanceKinematicsModel.getNewParamBySid(param); 508 509 if ( !newParam ) 510 { 511 continue; 512 } 513 514 KinematicsNewParam::ValueType newParamValueType = newParam->getValueType(); 515 if ( newParamValueType != KinematicsNewParam::VALUETYPE_SIDREF ) 516 { 517 continue; 518 } 519 520 const SidAddress& jointSidAddress = *newParam->getSidrefValue(); 521 522 const SidTreeNode* jointSidTreeNode = mDocumentProcessor->resolveSid(jointSidAddress); 523 524 if ( !jointSidTreeNode || (jointSidTreeNode->getTargetType() != SidTreeNode::TARGETTYPECLASS_OBJECT) ) 525 { 526 String msg = "Joint with sid address \"" + jointSidAddress.getSidAddressString() + 527 "\" referenced in kinematics model \"" + kinematicsModel->getUrl().getURIString() + "\" could not be found."; 528 //todo: handle error smarter 529 530 // mDocumentProcessor->handleFWLError(SaxFWLError::ERROR_UNRESOLVED_REFERENCE, msg); 531 continue; 532 } 533 534 COLLADAFW::Object* jointPrimitiveObject = jointSidTreeNode->getObjectTarget(); 535 COLLADAFW::JointPrimitive* jointPrimitive = COLLADAFW::objectSafeCast<COLLADAFW::JointPrimitive>( jointPrimitiveObject ); 536 537 if ( !jointPrimitive ) 538 { 539 String msg = "Element with sid address \"" + jointSidAddress.getSidAddressString() + 540 "\" referenced in kinematics model \"" + kinematicsModel->getUrl().getURIString() + "\" is not a joint primitive."; 541 mDocumentProcessor->handleFWLError(SaxFWLError::ERROR_UNEXPECTED_ELEMENT, msg); 542 continue; 543 } 544 545 KinematicsModelFWKinematicsModelMap::const_iterator it = mKinematicsModelFWKinematicsModelMap.find(kinematicsModel); 546 // there hast to be always a corresponding fw kin model 547 COLLADABU_ASSERT( it != mKinematicsModelFWKinematicsModelMap.end()); 548 549 COLLADAFW::KinematicsModel* fwKinematicsModel = it->second; 550 551 *linkNumber = findLinkByJOintPrimitive( fwKinematicsModel, jointPrimitive); 552 553 if ( *linkNumber == std::numeric_limits<size_t>::max() ) 554 { 555 continue; 556 } 557 558 *kinModel = fwKinematicsModel; 559 560 return true; 561 562 // instanceKinematicsModel->getKinematicsNewParams() 563 // kinematicsModel->getKinematicsInstanceKinematicsModels() 564 } 565 566 // const KinematicsInstanceArticulatedSystems& instanceArticulatedSystems = kinematicsScene->getKinematicsInstanceArticulatedSystems(); 567 // KinematicsInstanceArticulatedSystems::const_iterator it = instanceArticulatedSystems.begin(); 568 // for (; it != instanceArticulatedSystems.end(); ++it) 569 // { 570 // const KinematicsInstanceArticulatedSystem* instanceArticulatedSystem = *it; 571 // 572 // const COLLADABU::URI& articulatedSystemUrl = instanceArticulatedSystem->getUrl(); 573 // 574 // KinematicsController* controller = mDocumentProcessor->getKinematicsControllerByUri(articulatedSystemUrl); 575 // 576 // if ( !controller ) 577 // { 578 // String msg = "Articulated scene \"" + articulatedSystemUrl.getURIString() + "\" could not be found."; 579 // 580 // mDocumentProcessor->handleFWLError(SaxFWLError::ERROR_UNRESOLVED_REFERENCE, msg); 581 // continue; 582 // } 583 // 584 // controller->getKinematicsInstanceKinematicsModels() 585 // } 586 return false; 587 } 588 589 //------------------------------ findLinkByJOintPrimitive(const COLLADAFW::KinematicsModel * fwKinModel,const COLLADAFW::JointPrimitive * jointPrimitive)590 size_t KinematicsSceneCreator::findLinkByJOintPrimitive( const COLLADAFW::KinematicsModel* fwKinModel, const COLLADAFW::JointPrimitive* jointPrimitive ) 591 { 592 // find the joint containing the joint primitive 593 const COLLADAFW::JointPointerArray& joints = fwKinModel->getJoints(); 594 size_t joindIndex = std::numeric_limits<size_t>::max(); 595 const COLLADAFW::UniqueId& jointPrimitiveUniqueId = jointPrimitive->getUniqueId(); 596 size_t jointsCount = joints.getCount(); 597 for ( size_t i = 0; (i<jointsCount) && (joindIndex == std::numeric_limits<size_t>::max()); ++i ) 598 { 599 const COLLADAFW::Joint* joint = joints[i]; 600 const COLLADAFW::JointPrimitivePointerArray& jointPrimitives = joint->getJointPrimitives(); 601 size_t jointPrimitivesCount = jointPrimitives.getCount(); 602 for ( size_t j = 0; (j<jointPrimitivesCount) && (joindIndex == std::numeric_limits<size_t>::max()); ++j ) 603 { 604 const COLLADAFW::JointPrimitive* jointPrimitive2 = jointPrimitives[j]; 605 const COLLADAFW::UniqueId& jointPrimitiveUniqueId2 = jointPrimitive2->getUniqueId(); 606 if ( jointPrimitiveUniqueId == jointPrimitiveUniqueId2 ) 607 { 608 joindIndex = i; 609 } 610 } 611 } 612 613 // joint primitive could not be found in kin model 614 if ( joindIndex == std::numeric_limits<size_t>::max() ) 615 { 616 return std::numeric_limits<size_t>::max(); 617 } 618 619 // search all the link joint connection for the one with joint index joindIndex 620 const COLLADAFW::KinematicsModel::LinkJointConnections& linkJointConnections = fwKinModel->getLinkJointConnections(); 621 size_t linkJointConnectionsCount = linkJointConnections.getCount(); 622 size_t linkNumber = std::numeric_limits<size_t>::max(); 623 for ( size_t i = 0; i < linkJointConnectionsCount; ++i) 624 { 625 const COLLADAFW::KinematicsModel::LinkJointConnection* linkJointConnection = linkJointConnections[i]; 626 if ( linkJointConnection->getJointIndex() == joindIndex ) 627 { 628 linkNumber = std::min( linkNumber, linkJointConnection->getLinkNumber()); 629 } 630 } 631 632 return linkNumber; 633 } 634 635 } // namespace COLLADASaxFWL 636