RagdollDemo::RagdollDemo(hkDemoEnvironment* env) : hkDefaultPhysicsDemo(env) { // // Setup the camera // { hkVector4 from(5.0f, 2.0f, 5.0f); hkVector4 to(0.0f, 0.0f, 0.0f); hkVector4 up(0.0f, 1.0f, 0.0f); setupDefaultCameras( env, from, to, up ); } // // Create the world // { hkpWorldCinfo info; info.setupSolverInfo(hkpWorldCinfo::SOLVER_TYPE_4ITERS_MEDIUM); info.m_enableDeactivation = false; m_world = new hkpWorld( info ); m_world->lock(); // // Create constraint viewer // m_debugViewerNames.pushBack( hkpConstraintViewer::getName() ); hkpConstraintViewer::m_scale = 1.0f; setupGraphics(); // Register the single agent required (a hkpBoxBoxAgent) hkpAgentRegisterUtil::registerAllAgents( m_world->getCollisionDispatcher() ); } hkVector4 pivot(0.0f, 0.0f, 0.0f); hkVector4 halfSize(1.0f, 0.25f, 0.5f); // Create Box Shapes // hkpBoxShape* boxShape; { boxShape = new hkpBoxShape( halfSize , 0 ); } // // Create fixed rigid body // hkpRigidBody* fixedBody; { hkpRigidBodyCinfo info; info.m_position.set(-halfSize(0) - 1.0f, 0.0f, 0.0f); info.m_shape = boxShape; info.m_motionType = hkpMotion::MOTION_FIXED; fixedBody = new hkpRigidBody(info); m_world->addEntity(fixedBody); fixedBody->removeReference(); } // // Create movable rigid body // hkpRigidBody* moveableBody; { hkpRigidBodyCinfo info; info.m_position.set(halfSize(0) + 1.0f, 0.0f, 0.0f); info.m_shape = boxShape; // Compute the box inertia tensor hkpMassProperties massProperties; info.m_mass = 10.0f; hkpInertiaTensorComputer::computeBoxVolumeMassProperties(halfSize, info.m_mass, massProperties); info.m_inertiaTensor = massProperties.m_inertiaTensor; info.m_centerOfMass = massProperties.m_centerOfMass; info.m_motionType = hkpMotion::MOTION_BOX_INERTIA; moveableBody = new hkpRigidBody(info); m_world->addEntity(moveableBody); moveableBody->removeReference(); } // // Cleanup shape references // boxShape->removeReference(); boxShape = HK_NULL; // // CREATE RAGDOLL CONSTRAINT // { hkReal planeMin = HK_REAL_PI * -0.2f; hkReal planeMax = HK_REAL_PI * 0.1f; hkReal twistMin = HK_REAL_PI * -0.1f; hkReal twistMax = HK_REAL_PI * 0.4f; hkReal coneMin = HK_REAL_PI * 0.3f; hkpRagdollConstraintData* rdc = new hkpRagdollConstraintData(); rdc->setConeAngularLimit(coneMin); rdc->setPlaneMinAngularLimit(planeMin); rdc->setPlaneMaxAngularLimit(planeMax); rdc->setTwistMinAngularLimit(twistMin); rdc->setTwistMaxAngularLimit(twistMax); hkVector4 twistAxis(1.0f, 0.0f, 0.0f); hkVector4 planeAxis(0.0f, 1.0f, 0.0f); pivot.set(0.0f, 0.0f, 0.0f); rdc->setInWorldSpace(moveableBody->getTransform(), fixedBody->getTransform(), pivot, twistAxis, planeAxis); // // Create and add the constraint // { hkpConstraintInstance* constraint = new hkpConstraintInstance(moveableBody, fixedBody, rdc ); m_world->addConstraint(constraint); constraint->removeReference(); } rdc->removeReference(); } m_world->unlock(); }
void PrevailingWindDemo::createPalmTree ( hkpWorld* world, const hkpWind* wind, const hkVector4& pos ) { const hkReal trunkHeight = 4.0f; const hkReal trunkBottomRadius = 0.5f; const hkReal trunkTopRadius = 0.2f; const hkReal trunkStiffness = 0.1f; const hkReal segmentMass = 0.6f; const int numberOfSegments = 4; const hkReal segmentGap = 0.2f; const int numberOfFronds = 6; const hkReal frondWidth = 2.0f; const hkReal frondLength = 3.0f; const hkReal frondMass = 0.4f; // The trunk hkArray<hkpRigidBody*> trunk; const hkReal segmentHeight = (trunkHeight - ((numberOfSegments - 1) * segmentGap)) / numberOfSegments; const hkReal radiusIncrement = (trunkBottomRadius - trunkTopRadius) / numberOfSegments; for ( int i = 0; i < numberOfSegments; i++ ) { hkpShape* segmentShape; hkpRigidBodyCinfo info; { hkVector4 bottom( 0.0f, (segmentHeight + segmentGap) * i, 0.0f ); hkVector4 top( 0.0f, (segmentHeight + segmentGap) * i + segmentHeight, 0.0f ); hkReal radius = trunkBottomRadius - (radiusIncrement * i); segmentShape = new hkpCylinderShape( bottom, top, radius, 0.03f ); info.m_shape = segmentShape; info.m_position = pos; if (i == 0) { info.m_motionType = hkpMotion::MOTION_FIXED; } else { hkpMassProperties massProperties; { hkpInertiaTensorComputer::computeCylinderVolumeMassProperties( bottom, top, radius, segmentMass, massProperties ); } info.m_motionType = hkpMotion::MOTION_DYNAMIC; info.m_mass = massProperties.m_mass; info.m_inertiaTensor = massProperties.m_inertiaTensor; info.m_centerOfMass = massProperties.m_centerOfMass; } } hkpRigidBody* segment = new hkpRigidBody( info ); segmentShape->removeReference(); trunk.pushBack( segment ); world->addEntity( segment ); segment->removeReference(); if (i > 0) { hkpWindAction* action = new hkpWindAction( segment, wind, 0.1f ); world->addAction(action); action->removeReference(); } } for ( int i = 1; i < numberOfSegments; i++ ) { // We model the connection between the segments with a ragdoll constraint. hkpRagdollConstraintData* rdc; { hkReal planeMin = HK_REAL_PI * -0.025f; hkReal planeMax = HK_REAL_PI * 0.025f; hkReal twistMin = HK_REAL_PI * -0.025f; hkReal twistMax = HK_REAL_PI * 0.025f; hkReal coneMin = HK_REAL_PI * -0.05f; hkReal coneMax = HK_REAL_PI * 0.05f; rdc = new hkpRagdollConstraintData(); rdc->setPlaneMinAngularLimit( planeMin ); rdc->setPlaneMaxAngularLimit( planeMax ); rdc->setTwistMinAngularLimit( twistMin ); rdc->setTwistMaxAngularLimit( twistMax ); hkVector4 twistAxis( 0.0f, 1.0f, 0.0f ); hkVector4 planeAxis( 0.0f, 0.0f, 1.0f ); hkVector4 pivot( 0.0f, (segmentHeight + segmentGap) * i, 0.0f ); rdc->setInBodySpace( pivot, pivot, planeAxis, planeAxis, twistAxis, twistAxis ); rdc->setAsymmetricConeAngle( coneMin, coneMax ); //world->createAndAddConstraintInstance( trunk[i - 1], trunk[i], rdc )->removeReference(); hkpConstraintInstance* constraint = new hkpConstraintInstance( trunk[i - 1], trunk[i], rdc ); world->addConstraint(constraint); hkpPositionConstraintMotor* motor = new hkpPositionConstraintMotor( 0 ); motor->m_tau = trunkStiffness; motor->m_maxForce = 1000.0f; motor->m_constantRecoveryVelocity = 0.1f; rdc->setTwistMotor( motor ); rdc->setConeMotor( motor ); rdc->setPlaneMotor( motor ); rdc->setMotorsActive(constraint, true); motor->removeReference(); constraint->removeReference(); rdc->removeReference(); } } // The angle that the leaves make with the ground in their half lifted position. hkQuaternion tilt; { hkVector4 axis( 0.0f, 0.0f, 1.0f ); tilt.setAxisAngle( axis, HK_REAL_PI * 0.1f ); } hkQuaternion tiltRot; // The fronds for ( int i = 0; i < numberOfFronds; i++ ) { hkQuaternion rotation; { hkVector4 axis( 0.0f, 1.0f, 0.0f ); rotation.setAxisAngle( axis, HK_REAL_PI * 2.0f * ( i / (hkReal) numberOfFronds ) ); rotation.normalize(); } hkpShape* frondShape; hkpRigidBodyCinfo info; { hkVector4 vertexA( 0.0f, 0.0f, 0.0f ); hkVector4 vertexB( frondLength, 0.0f, frondWidth / 2.0f ); hkVector4 vertexC( frondLength, 0.0f, - frondWidth / 2.0f ); frondShape = new hkpTriangleShape( vertexA, vertexB, vertexC, 0.01f ); info.m_shape = frondShape; hkVector4 relPos; relPos.setRotatedDir( rotation, hkVector4( trunkTopRadius + 0.3f, trunkHeight, 0.0f ) ); info.m_position.setAdd4( pos, relPos ); hkpMassProperties massProperties; { hkReal mass = frondMass; hkpInertiaTensorComputer::computeTriangleSurfaceMassProperties( vertexA, vertexB, vertexC, mass, 0.01f, massProperties ); } info.m_motionType = hkpMotion::MOTION_DYNAMIC; info.m_mass = massProperties.m_mass; info.m_inertiaTensor = massProperties.m_inertiaTensor; info.m_centerOfMass = massProperties.m_centerOfMass; tiltRot.setMul( rotation, tilt ); info.m_rotation = tiltRot; } hkpRigidBody* frond = new hkpRigidBody( info ); frondShape->removeReference(); world->addEntity( frond ); hkpWindAction* action = new hkpWindAction( frond, wind, 0.1f ); world->addAction(action); action->removeReference(); // We model the connection between the fronds and the trunk with a ragdoll constraint. hkpRagdollConstraintData* rdc; { hkReal planeMin = HK_REAL_PI * -0.005f; hkReal planeMax = HK_REAL_PI * 0.005f; hkReal twistMin = HK_REAL_PI * -0.05f; hkReal twistMax = HK_REAL_PI * 0.05f; hkReal coneMin = HK_REAL_PI * -0.2f; hkReal coneMax = HK_REAL_PI * 0.2f; rdc = new hkpRagdollConstraintData(); rdc->setPlaneMinAngularLimit( planeMin ); rdc->setPlaneMaxAngularLimit( planeMax ); rdc->setTwistMinAngularLimit( twistMin ); rdc->setTwistMaxAngularLimit( twistMax ); hkVector4 twistAxisFrond( 1.0f, 0.0f, 0.0f ); hkVector4 twistAxisTrunk; twistAxisTrunk.setRotatedDir( tiltRot, twistAxisFrond ); hkVector4 planeAxisFrond( 0.0f, 0.0f, 1.0f ); hkVector4 planeAxisTrunk; planeAxisTrunk.setRotatedDir( tiltRot, planeAxisFrond ); hkVector4 pivotFrond( 0.0f, 0.0f, 0.0f ); hkVector4 pivotTrunk; pivotTrunk.setRotatedDir( rotation, hkVector4( trunkTopRadius + 0.3f, trunkHeight, 0.0f ) ); rdc->setInBodySpace( pivotTrunk, pivotFrond, planeAxisTrunk, planeAxisFrond, twistAxisTrunk, twistAxisFrond ); rdc->setAsymmetricConeAngle( coneMin, coneMax ); world->createAndAddConstraintInstance( trunk[ numberOfSegments - 1 ], frond, rdc )->removeReference(); rdc->removeReference(); frond->removeReference(); } } }
PoweredRagdollDemo::PoweredRagdollDemo(hkDemoEnvironment* env) : hkDefaultPhysicsDemo(env) { m_time = 0.0f; // // Setup the camera // { hkVector4 from(5.0f, 2.0f, 5.0f); hkVector4 to(0.0f, 0.0f, 0.0f); hkVector4 up(0.0f, 1.0f, 0.0f); setupDefaultCameras( env, from, to, up ); } // // Create the world // { hkpWorldCinfo info; info.setupSolverInfo(hkpWorldCinfo::SOLVER_TYPE_8ITERS_MEDIUM); info.m_enableDeactivation = false; m_world = new hkpWorld( info ); m_world->lock(); // // Create constraint viewer // m_debugViewerNames.pushBack( hkpConstraintViewer::getName() ); hkpConstraintViewer::m_scale = 1.0f; setupGraphics(); // Register the single agent required (a hkpBoxBoxAgent) hkpAgentRegisterUtil::registerAllAgents( m_world->getCollisionDispatcher() ); } // // Create vectors to be used for setup // hkVector4 pivot(0.0f, 0.0f, 0.0f); hkVector4 halfSize(1.0f, 0.25f, 0.5f); // // Create Box Shape // hkpBoxShape* boxShape; { boxShape = new hkpBoxShape( halfSize , 0 ); } // // Create fixed rigid body // hkpRigidBody* fixedBody; { hkpRigidBodyCinfo info; info.m_position.set(-halfSize(0) - 1.0f, 0.0f, 0.0f); info.m_shape = boxShape; info.m_motionType = hkpMotion::MOTION_FIXED; fixedBody = new hkpRigidBody(info); m_world->addEntity(fixedBody); fixedBody->removeReference(); } // // Create movable rigid body // hkpRigidBody* moveableBody; { hkpRigidBodyCinfo info; info.m_position.set(halfSize(0) + 1.0f, 0.0f, 0.0f); info.m_shape = boxShape; // Compute the box inertia tensor hkpMassProperties massProperties; info.m_mass = 10.0f; hkpInertiaTensorComputer::computeBoxVolumeMassProperties(halfSize, info.m_mass, massProperties); info.m_inertiaTensor = massProperties.m_inertiaTensor; info.m_centerOfMass = massProperties.m_centerOfMass; info.m_motionType = hkpMotion::MOTION_BOX_INERTIA; moveableBody = new hkpRigidBody(info); m_world->addEntity(moveableBody); moveableBody->removeReference(); } // // Cleanup shape references // boxShape->removeReference(); boxShape = HK_NULL; // // CREATE POWERED RAGDOLL CONSTRAINT // { hkReal planeMin = HK_REAL_PI * -0.2f; hkReal planeMax = HK_REAL_PI * 0.1f; hkReal twistMin = HK_REAL_PI * -0.1f; hkReal twistMax = HK_REAL_PI * 0.4f; hkReal coneMin = HK_REAL_PI * 0.3f; hkpRagdollConstraintData* rdc = new hkpRagdollConstraintData(); rdc->setConeAngularLimit(coneMin); rdc->setPlaneMinAngularLimit(planeMin); rdc->setPlaneMaxAngularLimit(planeMax); rdc->setTwistMinAngularLimit(twistMin); rdc->setTwistMaxAngularLimit(twistMax); hkVector4 twistAxis(1.0f, 0.0f, 0.0f); hkVector4 planeAxis(0.0f, 1.0f, 0.0f); pivot.set(0.0f, 0.0f, 0.0f); rdc->setInWorldSpace(moveableBody->getTransform(), fixedBody->getTransform(), pivot, twistAxis, planeAxis); // // Create and add the constraint // hkpConstraintInstance* constraint = new hkpConstraintInstance(moveableBody, fixedBody, rdc ); m_world->addConstraint(constraint); hkpPositionConstraintMotor* motor = new hkpPositionConstraintMotor( 0 ); motor->m_tau = 0.6f; motor->m_maxForce = 1000.0f; motor->m_constantRecoveryVelocity = 0.1f; rdc->setTwistMotor( motor ); rdc->setConeMotor( motor ); rdc->setPlaneMotor( motor ); rdc->setMotorsActive(constraint, true); motor->removeReference(); hkRotation bRa; bRa.setTranspose( fixedBody->getTransform().getRotation() ); bRa.mul( moveableBody->getTransform().getRotation() ); rdc->setTargetRelativeOrientationOfBodies( bRa ); m_constraintData = rdc; constraint->removeReference(); } // // So that we can actually see the motor in action, rotate the constrained body slightly. // When simulation starts it will then by driven to the target position (frame). // { hkQuaternion rot; hkVector4 axis( 1.0f, 0.0f, 0.0f ); axis.normalize3(); rot.setAxisAngle( axis, 0.4f ); moveableBody->setRotation( rot ); } m_world->unlock(); }
void MakeKukaRobot(DemoEntityManager* const scene, DemoEntity* const model) { m_kinematicSolver = NewtonCreateInverseDynamics(scene->GetNewton()); NewtonBody* const baseFrameBody = CreateBodyPart(model, armRobotConfig[0]); void* const baseFrameNode = NewtonInverseDynamicsAddRoot(m_kinematicSolver, baseFrameBody); NewtonBodySetMassMatrix(baseFrameBody, 0.0f, 0.0f, 0.0f, 0.0f); dMatrix boneAligmentMatrix( dVector(0.0f, 1.0f, 0.0f, 0.0f), dVector(0.0f, 0.0f, 1.0f, 0.0f), dVector(1.0f, 0.0f, 0.0f, 0.0f), dVector(0.0f, 0.0f, 0.0f, 1.0f)); int stackIndex = 0; DemoEntity* childEntities[32]; void* parentBones[32]; for (DemoEntity* child = model->GetChild(); child; child = child->GetSibling()) { parentBones[stackIndex] = baseFrameNode; childEntities[stackIndex] = child; stackIndex++; } dKukaEffector* effector = NULL; const int partCount = sizeof(armRobotConfig) / sizeof(armRobotConfig[0]); while (stackIndex) { stackIndex--; DemoEntity* const entity = childEntities[stackIndex]; void* const parentJoint = parentBones[stackIndex]; const char* const name = entity->GetName().GetStr(); for (int i = 0; i < partCount; i++) { if (!strcmp(armRobotConfig[i].m_partName, name)) { if (strstr(name, "bone")) { // add a bone and all it children dMatrix matrix; NewtonBody* const limbBody = CreateBodyPart(entity, armRobotConfig[i]); NewtonBodyGetMatrix(limbBody, &matrix[0][0]); NewtonBody* const parentBody = NewtonInverseDynamicsGetBody(m_kinematicSolver, parentJoint); dCustomInverseDynamics* const rotatingColumnHinge = new dCustomInverseDynamics(boneAligmentMatrix * matrix, limbBody, parentBody); rotatingColumnHinge->SetJointTorque(armRobotConfig[i].m_mass * DEMO_GRAVITY * 50.0f); rotatingColumnHinge->SetTwistAngle(armRobotConfig[i].m_minLimit * dDegreeToRad, armRobotConfig[i].m_maxLimit * dDegreeToRad); void* const limbJoint = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, parentJoint, rotatingColumnHinge->GetJoint()); for (DemoEntity* child = entity->GetChild(); child; child = child->GetSibling()) { parentBones[stackIndex] = limbJoint; childEntities[stackIndex] = child; stackIndex++; } } else if (strstr(name, "effector")) { // add effector dMatrix gripperMatrix(entity->CalculateGlobalMatrix()); effector = new dKukaEffector(m_kinematicSolver, parentJoint, baseFrameBody, gripperMatrix); effector->SetAsThreedof(); effector->SetLinearSpeed(2.0f); effector->SetMaxLinearFriction(armRobotConfig[i].m_mass * DEMO_GRAVITY * 50.0f); } break; } } } // create the Animation tree for manipulation DemoEntity* const effectoBone = model->Find("effector_arm"); dMatrix baseFrameMatrix(model->GetCurrentMatrix()); dMatrix effectorLocalMatrix(effectoBone->CalculateGlobalMatrix(model)); dVector upAxis(baseFrameMatrix.UnrotateVector(dVector(0.0f, 1.0f, 0.0f, 1.0f))); dVector planeAxis(baseFrameMatrix.UnrotateVector(dVector(0.0f, 0.0f, 1.0f, 1.0f))); dEffectorTreeFixPose* const fixPose = new dEffectorTreeFixPose(baseFrameBody); m_inputModifier = new dEffectorTreeInputModifier(fixPose, upAxis, planeAxis); m_animTreeNode = new dEffectorTreeRoot(baseFrameBody, m_inputModifier); // set base pose dEffectorTreeInterface::dEffectorTransform frame; frame.m_effector = effector; frame.m_posit = effectorLocalMatrix.m_posit; frame.m_rotation = dQuaternion(effectorLocalMatrix); fixPose->GetPose().Append(frame); m_animTreeNode->GetPose().Append(frame); NewtonInverseDynamicsEndBuild(m_kinematicSolver); }