Example #1
0
void DG_Rotatef(float angle, float x, float y, float z)
{
    D3DXVECTOR3 axis(x, y, z);
    matStack[msIndex]->RotateAxisLocal(&axis, D3DXToRadian(angle));
    UploadMatrix();
}
Example #2
0
mat3d rotationFromAzElTwist(double az, double el, double twist)
{
    vec3d axis(cos(az), 0.0, -sin(az));
    return vmath::rotation_matrix3(el, axis) * vmath::rotation_matrix3(twist + az, unitY);
}
Example #3
0
void ConstraintDemo::clientMoveAndDisplay()
{
	
 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

 	float dt = float(getDeltaTimeMicroseconds()) * 0.000001f;
	//printf("dt = %f: ",dt);

	// drive cone-twist motor
	m_Time += 0.03f;
	if (s_bTestConeTwistMotor)
	{  // this works only for obsolete constraint solver for now
		// build cone target
		btScalar t = 1.25f*m_Time;
		btVector3 axis(0,sin(t),cos(t));
		axis.normalize();
		btQuaternion q1(axis, 0.75f*SIMD_PI);

		// build twist target
		//btQuaternion q2(0,0,0);
		//btQuaternion q2(btVehictor3(1,0,0), -0.3*sin(m_Time));
		btQuaternion q2(btVector3(1,0,0), -1.49f*btSin(1.5f*m_Time));

		// compose cone + twist and set target
		q1 = q1 * q2;
		m_ctc->enableMotor(true);
		m_ctc->setMotorTargetInConstraintSpace(q1);
	}

	{
		static bool once = true;
		if ( m_dynamicsWorld->getDebugDrawer() && once)
		{
			m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits);
			once=false;
		}
	}

	
	{
	 	//during idle mode, just run 1 simulation step maximum
		int maxSimSubSteps = m_idle ? 1 : 1;
		if (m_idle)
			dt = 1.0f/420.f;

		int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);

		//optional but useful: debug drawing
		m_dynamicsWorld->debugDrawWorld();
	
		bool verbose = false;
		if (verbose)
		{
			if (!numSimSteps)
				printf("Interpolated transforms\n");
			else
			{
				if (numSimSteps > maxSimSubSteps)
				{
					//detect dropping frames
					printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps);
				} else
				{
					printf("Simulated (%i) steps\n",numSimSteps);
				}
			}
		}
	}
	renderme();

//	drawLimit();

    glFlush();
    swapBuffers();
}
Example #4
0
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();
		}
	}
}
Example #5
0
void smurf::Robot::loadFromSmurf(const std::string& path)
{
    configmaps::ConfigMap map;

    // parse joints from model
    boost::filesystem::path filepath(path);
    model = smurf_parser::parseFile(&map, filepath.parent_path().generic_string(), filepath.filename().generic_string(), true);
    
    //first we need to create all Frames
    for (configmaps::ConfigVector::iterator it = map["frames"].begin(); it != map["frames"].end(); ++it) 
    {
        configmaps::ConfigMap &fr(it->children);

        Frame *frame = new Frame(fr["name"]);
        availableFrames.push_back(frame);
        //std::cout << "Adding additional frame " << frame->getName() << std::endl;
    }
    
    for(std::pair<std::string, boost::shared_ptr<urdf::Link>> link: model->links_)
    {
        Frame *frame = new Frame(link.first);
        for(boost::shared_ptr<urdf::Visual> visual : link.second->visual_array)
        {
            frame->addVisual(*visual);
        }
        availableFrames.push_back(frame);
        

        //std::cout << "Adding link frame " << frame->getName() << std::endl;
    }

    for(std::pair<std::string, boost::shared_ptr<urdf::Joint> > jointIt: model->joints_)
    {
        boost::shared_ptr<urdf::Joint> joint = jointIt.second;
        //std::cout << "Adding joint " << joint->name << std::endl;
        
        Frame *source = getFrameByName(joint->parent_link_name);
        Frame *target = getFrameByName(joint->child_link_name);

        //TODO this might not be set in some cases, perhaps force a check
        configmaps::ConfigMap annotations;
        for(configmaps::ConfigItem &cv : map["joints"])
        {
            if(static_cast<std::string>(cv.children["name"]) == joint->name)
            {
                annotations = cv.children;
            }
        }
        switch(joint->type)
        {
            case urdf::Joint::FIXED:
            {
                const urdf::Pose &tr(joint->parent_to_joint_origin_transform);     
                StaticTransformation *transform = new StaticTransformation(source, target,
                                                                           Eigen::Quaterniond(tr.rotation.w, tr.rotation.x, tr.rotation.y, tr.rotation.z),
                                                                           Eigen::Vector3d(tr.position.x, tr.position.y, tr.position.z));              
                staticTransforms.push_back(transform);
            }
            break;
            case urdf::Joint::FLOATING:
            {
                DynamicTransformation *transform = new DynamicTransformation(source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"));
                dynamicTransforms.push_back(transform);
                Eigen::Vector3d axis(joint->axis.x, joint->axis.y, joint->axis.z);
                Eigen::Affine3d sourceToAxis(Eigen::Affine3d::Identity());
                sourceToAxis.translation() = axis;
                base::JointLimitRange limits;
                const urdf::Pose parentToOrigin(joint->parent_to_joint_origin_transform);
                Eigen::Quaterniond rot(parentToOrigin.rotation.w, parentToOrigin.rotation.x, parentToOrigin.rotation.y, parentToOrigin.rotation.z);
                Eigen::Vector3d trans(parentToOrigin.position.x, parentToOrigin.position.y, parentToOrigin.position.z);
                Eigen::Affine3d parentToOriginAff;
                parentToOriginAff.setIdentity();
                parentToOriginAff.rotate(rot);
                parentToOriginAff.translation() = trans;
                Joint *smurfJoint = new Joint (source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"), checkGet(annotations, "driver"), limits, sourceToAxis, parentToOriginAff, joint); 
                joints.push_back(smurfJoint);
            }
            break;
            case urdf::Joint::REVOLUTE:
            case urdf::Joint::CONTINUOUS:
            case urdf::Joint::PRISMATIC:
            {
                base::JointState minState;
                minState.position = joint->limits->lower;
                minState.effort = 0;
                minState.speed = 0;
                
                base::JointState maxState;
                maxState.position = joint->limits->upper;
                maxState.effort = joint->limits->effort;
                maxState.speed = joint->limits->velocity;
                
                base::JointLimitRange limits;
                limits.min = minState;
                limits.max = maxState;
                
                Eigen::Vector3d axis(joint->axis.x, joint->axis.y, joint->axis.z);
                Eigen::Affine3d sourceToAxis(Eigen::Affine3d::Identity());
                sourceToAxis.translation() = axis;
                
                DynamicTransformation *transform = NULL;
                Joint *smurfJoint;
                // push the correspondent smurf::joint 
                const urdf::Pose parentToOrigin(joint->parent_to_joint_origin_transform);
                Eigen::Quaterniond rot(parentToOrigin.rotation.w, parentToOrigin.rotation.x, parentToOrigin.rotation.y, parentToOrigin.rotation.z);
                Eigen::Vector3d trans(parentToOrigin.position.x, parentToOrigin.position.y, parentToOrigin.position.z);
                Eigen::Affine3d parentToOriginAff;
                parentToOriginAff.setIdentity();
                parentToOriginAff.rotate(rot);
                parentToOriginAff.translation() = trans;
                if(joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::CONTINUOUS)
                {
                    transform = new RotationalJoint(source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"), checkGet(annotations, "driver"), limits, sourceToAxis, axis, parentToOriginAff, joint);
                    smurfJoint = (Joint *)transform;
                }
                else
                {
                    transform = new TranslationalJoint(source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"), checkGet(annotations, "driver"), limits, sourceToAxis, axis, parentToOriginAff, joint);
                    smurfJoint = (Joint *)transform;
                }
                dynamicTransforms.push_back(transform);
                joints.push_back(smurfJoint);
            }
            break;
            default:
                throw std::runtime_error("Smurf: Error, got unhandles Joint type");
        }
    }

    
    // parse sensors from map
    for (configmaps::ConfigVector::iterator it = map["sensors"].begin(); it != map["sensors"].end(); ++it) 
    {
        configmaps::ConfigMap sensorMap = it->children;
        smurf::Sensor *sensor = new Sensor(sensorMap["name"], sensorMap["type"], sensorMap["taskInstanceName"], getFrameByName(sensorMap["link"]));
        sensors.push_back(sensor);
    }
}
Example #6
0
int runCTest( const std::string& testName )
{
    const std::string fileName( "testconstraint.osg" );

    // Create two rigid bodies for testing.

    osg::ref_ptr< osg::Group > root = new osg::Group;

    osg::Node* node = osgDB::readNodeFile( "tetra.osg" );
    if( node == NULL )
        ERROR("Init:","Can't load model data file.");
    osg::Matrix aXform = osg::Matrix::translate( 4., 2., 0. );

    osgwTools::AbsoluteModelTransform* amt = new osgwTools::AbsoluteModelTransform;
    amt->setDataVariance( osg::Object::DYNAMIC );
    amt->addChild( node );
    root->addChild( amt );

    osg::ref_ptr< osgbDynamics::CreationRecord > cr = new osgbDynamics::CreationRecord;
    cr->_sceneGraph = amt;
    cr->_shapeType = BOX_SHAPE_PROXYTYPE;
        cr->_mass = .5;
    cr->_parentTransform = aXform;
    btRigidBody* rbA = osgbDynamics::createRigidBody( cr.get() );


    node = osgDB::readNodeFile( "block.osg" );
    if( node == NULL )
        ERROR("Init:","Can't load model data file.");
    osg::Matrix bXform = osg::Matrix::identity();

    amt = new osgwTools::AbsoluteModelTransform;
    amt->setDataVariance( osg::Object::DYNAMIC );
    amt->addChild( node );
    root->addChild( amt );

    cr = new osgbDynamics::CreationRecord;
    cr->_sceneGraph = amt;
    cr->_shapeType = BOX_SHAPE_PROXYTYPE;
        cr->_mass = 4.;
    cr->_parentTransform = bXform;
    btRigidBody* rbB = osgbDynamics::createRigidBody( cr.get() );

    //
    // SliderConstraint
    if( testName == std::string( "Slider" ) )
    {
        osg::Vec3 axis( 0., 0., 1. );
        osg::Vec2 limits( -4., 4. );
        osg::ref_ptr< osgbDynamics::SliderConstraint > cons = new osgbDynamics::SliderConstraint(
            rbA, aXform, rbB, bXform, axis, limits );

        if( cons->getAsBtSlider() == NULL )
            ERROR(testName,"won't typecast as btSliderConstraint.");

        if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
            ERROR(testName,"writeObjectFile failed.");

        osg::Object* obj = osgDB::readObjectFile( fileName );
        if( obj == NULL )
            ERROR(testName,"readObjectFile returned NULL.");

        osg::ref_ptr< osgbDynamics::SliderConstraint > cons2 = dynamic_cast<
            osgbDynamics::SliderConstraint* >( obj );
        if( !( cons2.valid() ) )
            ERROR(testName,"dynamic_cast after readObjectFile failed.");

        if( *cons2 != *cons )
            // Note matches can fail due to double precision roundoff.
            // For testing, use only 1s and 0s in matrices.
            ERROR(testName,"failed to match.");

        return( 0 );
    }

    //
    // TwistSliderConstraint
    if( testName == std::string( "TwistSlider" ) )
    {
        osg::Vec3 axis( 0., 0., 1. );
        osg::Vec3 point( 1., 2., 3. );
        osg::Vec2 linLimits( -4., 4. );
        osg::Vec2 angLimits( -1., 2. );
        osg::ref_ptr< osgbDynamics::TwistSliderConstraint > cons = new osgbDynamics::TwistSliderConstraint(
            rbA, aXform, rbB, bXform, axis, point, linLimits, angLimits );

        if( cons->getAsBtSlider() == NULL )
            ERROR(testName,"won't typecast as btSliderConstraint.");

        if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
            ERROR(testName,"writeObjectFile failed.");

        osg::Object* obj = osgDB::readObjectFile( fileName );
        if( obj == NULL )
            ERROR(testName,"readObjectFile returned NULL.");

        osg::ref_ptr< osgbDynamics::TwistSliderConstraint > cons2 = dynamic_cast<
            osgbDynamics::TwistSliderConstraint* >( obj );
        if( !( cons2.valid() ) )
            ERROR(testName,"dynamic_cast after readObjectFile failed.");

        if( *cons2 != *cons )
            // Note matches can fail due to double precision roundoff.
            // For testing, use only 1s and 0s in matrices.
            ERROR(testName,"failed to match.");

        return( 0 );
    }

    //
    // BallAndSocketConstraint
    if( testName == std::string( "BallAndSocket" ) )
    {
        osg::Vec3 point( -5., 5., 3. );
        osg::ref_ptr< osgbDynamics::BallAndSocketConstraint > cons = new osgbDynamics::BallAndSocketConstraint(
            rbA, aXform, rbB, bXform, point );

        if( cons->getAsBtPoint2Point() == NULL )
            ERROR(testName,"won't typecast as btPoint2PointConstraint.");

        if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
            ERROR(testName,"writeObjectFile failed.");

        osg::Object* obj = osgDB::readObjectFile( fileName );
        if( obj == NULL )
            ERROR(testName,"readObjectFile returned NULL.");

        osg::ref_ptr< osgbDynamics::BallAndSocketConstraint > cons2 = dynamic_cast<
            osgbDynamics::BallAndSocketConstraint* >( obj );
        if( !( cons2.valid() ) )
            ERROR(testName,"dynamic_cast after readObjectFile failed.");

        if( *cons2 != *cons )
            // Note matches can fail due to double precision roundoff.
            // For testing, use only 1s and 0s in matrices.
            ERROR(testName,"failed to match.");

        return( 0 );
    }

    //
    // FixedConstraint
    if( testName == std::string( "Fixed" ) )
    {
        osg::ref_ptr< osgbDynamics::FixedConstraint > cons = new osgbDynamics::FixedConstraint(
            rbA, aXform, rbB, bXform );

        if( cons->getAsBtGeneric6Dof() == NULL )
            ERROR(testName,"won't typecast as btGeneric6DofConstraint.");

        if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
            ERROR(testName,"writeObjectFile failed.");

        osg::Object* obj = osgDB::readObjectFile( fileName );
        if( obj == NULL )
            ERROR(testName,"readObjectFile returned NULL.");

        osg::ref_ptr< osgbDynamics::FixedConstraint > cons2 = dynamic_cast<
            osgbDynamics::FixedConstraint* >( obj );
        if( !( cons2.valid() ) )
            ERROR(testName,"dynamic_cast after readObjectFile failed.");

        if( *cons2 != *cons )
            // Note matches can fail due to double precision roundoff.
            // For testing, use only 1s and 0s in matrices.
            ERROR(testName,"failed to match.");

        return( 0 );
    }

    //
    // PlanarConstraint
    if( testName == std::string( "Planar" ) )
    {
        osg::Vec2 loLimit( -2., -3. );
        osg::Vec2 hiLimit( 1., 4. );
        osg::Matrix orient( osg::Matrix::identity() );
        osg::ref_ptr< osgbDynamics::PlanarConstraint > cons = new osgbDynamics::PlanarConstraint(
            rbA, aXform, rbB, bXform, loLimit, hiLimit, orient );

        if( cons->getAsBtGeneric6Dof() == NULL )
            ERROR(testName,"won't typecast as btGeneric6DofConstraint.");

        if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
            ERROR(testName,"writeObjectFile failed.");

        osg::Object* obj = osgDB::readObjectFile( fileName );
        if( obj == NULL )
            ERROR(testName,"readObjectFile returned NULL.");

        osg::ref_ptr< osgbDynamics::PlanarConstraint > cons2 = dynamic_cast<
            osgbDynamics::PlanarConstraint* >( obj );
        if( !( cons2.valid() ) )
            ERROR(testName,"dynamic_cast after readObjectFile failed.");

        if( *cons2 != *cons )
            // Note matches can fail due to double precision roundoff.
            // For testing, use only 1s and 0s in matrices.
            ERROR(testName,"failed to match.");

        return( 0 );
    }

    //
    // BoxConstraint
    if( testName == std::string( "Box" ) )
    {
        osg::Vec3 loLimit( -2., -3., 0. );
        osg::Vec3 hiLimit( 1., 4., 2. );
        osg::Matrix orient( osg::Matrix::identity() );
        osg::ref_ptr< osgbDynamics::BoxConstraint > cons = new osgbDynamics::BoxConstraint(
            rbA, aXform, rbB, bXform, loLimit, hiLimit, orient );

        if( cons->getAsBtGeneric6Dof() == NULL )
            ERROR(testName,"won't typecast as btGeneric6DofConstraint.");

        if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
            ERROR(testName,"writeObjectFile failed.");

        osg::Object* obj = osgDB::readObjectFile( fileName );
        if( obj == NULL )
            ERROR(testName,"readObjectFile returned NULL.");

        osg::ref_ptr< osgbDynamics::BoxConstraint > cons2 = dynamic_cast<
            osgbDynamics::BoxConstraint* >( obj );
        if( !( cons2.valid() ) )
            ERROR(testName,"dynamic_cast after readObjectFile failed.");

        if( *cons2 != *cons )
            // Note matches can fail due to double precision roundoff.
            // For testing, use only 1s and 0s in matrices.
            ERROR(testName,"failed to match.");

        return( 0 );
    }

    //
    // LinearSpringConstraint
    if( testName == std::string( "LinearSpring" ) )
    {
        osg::ref_ptr< osgbDynamics::LinearSpringConstraint > cons = new osgbDynamics::LinearSpringConstraint(
            rbA, aXform, rbB, bXform, osg::Vec3( 2., 1., 0. ) );
        cons->setLimit( osg::Vec2( -2., 3. ) );
        cons->setStiffness( 40.f );
        cons->setDamping( .5f );

        if( cons->getAsBtGeneric6DofSpring() == NULL )
            ERROR(testName,"won't typecast as btGeneric6DofSpringConstraint.");

        if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
            ERROR(testName,"writeObjectFile failed.");

        osg::Object* obj = osgDB::readObjectFile( fileName );
        if( obj == NULL )
            ERROR(testName,"readObjectFile returned NULL.");

        osg::ref_ptr< osgbDynamics::LinearSpringConstraint > cons2 = dynamic_cast<
            osgbDynamics::LinearSpringConstraint* >( obj );
        if( !( cons2.valid() ) )
            ERROR(testName,"dynamic_cast after readObjectFile failed.");

        if( *cons2 != *cons )
            // Note matches can fail due to double precision roundoff.
            // For testing, use only 1s and 0s in matrices.
            ERROR(testName,"failed to match.");

        return( 0 );
    }

    //
    // AngleSpringConstraint
    if( testName == std::string( "AngleSpring" ) )
    {
        osg::ref_ptr< osgbDynamics::AngleSpringConstraint > cons = new osgbDynamics::AngleSpringConstraint(
            rbA, aXform, rbB, bXform, osg::Vec3( 2., 1., 0. ), osg::Vec3( 5., 6., -7. ) );
        cons->setLimit( osg::Vec2( -2., 1. ) );
        cons->setStiffness( 50.f );
        cons->setDamping( 0.f );

        if( cons->getAsBtGeneric6DofSpring() == NULL )
            ERROR(testName,"won't typecast as btGeneric6DofSpringConstraint.");

        if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
            ERROR(testName,"writeObjectFile failed.");

        osg::Object* obj = osgDB::readObjectFile( fileName );
        if( obj == NULL )
            ERROR(testName,"readObjectFile returned NULL.");

        osg::ref_ptr< osgbDynamics::AngleSpringConstraint > cons2 = dynamic_cast<
            osgbDynamics::AngleSpringConstraint* >( obj );
        if( !( cons2.valid() ) )
            ERROR(testName,"dynamic_cast after readObjectFile failed.");

        if( *cons2 != *cons )
            // Note matches can fail due to double precision roundoff.
            // For testing, use only 1s and 0s in matrices.
            ERROR(testName,"failed to match.");

        return( 0 );
    }

    //
    // LinearAngleSpringConstraint
    if( testName == std::string( "LinearAngleSpring" ) )
    {
        osg::ref_ptr< osgbDynamics::LinearAngleSpringConstraint > cons = new osgbDynamics::LinearAngleSpringConstraint(
            rbA, aXform, rbB, bXform, osg::Vec3( 2., 1., 0. ), osg::Vec3( 5., 6., -7. ) );
        cons->setLinearLimit( osg::Vec2( -2., 2. ) );
        cons->setAngleLimit( osg::Vec2( -3., 3. ) );
        cons->setLinearStiffness( 41.f );
        cons->setLinearDamping( 1.f );
        cons->setAngleStiffness( 42.f );
        cons->setAngleDamping( 2.f );

        if( cons->getAsBtGeneric6DofSpring() == NULL )
            ERROR(testName,"won't typecast as btGeneric6DofSpringConstraint.");

        if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
            ERROR(testName,"writeObjectFile failed.");

        osg::Object* obj = osgDB::readObjectFile( fileName );
        if( obj == NULL )
            ERROR(testName,"readObjectFile returned NULL.");

        osg::ref_ptr< osgbDynamics::LinearAngleSpringConstraint > cons2 = dynamic_cast<
            osgbDynamics::LinearAngleSpringConstraint* >( obj );
        if( !( cons2.valid() ) )
            ERROR(testName,"dynamic_cast after readObjectFile failed.");

        if( *cons2 != *cons )
            // Note matches can fail due to double precision roundoff.
            // For testing, use only 1s and 0s in matrices.
            ERROR(testName,"failed to match.");

        return( 0 );
    }

    //
    // HingeConstraint
    if( testName == std::string( "Hinge" ) )
    {
        osg::Vec3 axis( -1., 0., 0. );
        osg::Vec3 point( 4., 3., 2. );
        osg::Vec2 limit( -2., 2. );
        osg::ref_ptr< osgbDynamics::HingeConstraint > cons = new osgbDynamics::HingeConstraint(
            rbA, aXform, rbB, bXform, axis, point, limit );

        if( cons->getAsBtHinge() == NULL )
            ERROR(testName,"won't typecast as btHingeConstraint.");

        if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
            ERROR(testName,"writeObjectFile failed.");

        osg::Object* obj = osgDB::readObjectFile( fileName );
        if( obj == NULL )
            ERROR(testName,"readObjectFile returned NULL.");

        osg::ref_ptr< osgbDynamics::HingeConstraint > cons2 = dynamic_cast<
            osgbDynamics::HingeConstraint* >( obj );
        if( !( cons2.valid() ) )
            ERROR(testName,"dynamic_cast after readObjectFile failed.");

        if( *cons2 != *cons )
            // Note matches can fail due to double precision roundoff.
            // For testing, use only 1s and 0s in matrices.
            ERROR(testName,"failed to match.");

        return( 0 );
    }

    //
    // CardanConstraint
    if( testName == std::string( "Cardan" ) )
    {
        osg::Vec3 axisA( -1., 0., 0. );
        osg::Vec3 axisB( 0., 0., 1. );
        osg::ref_ptr< osgbDynamics::CardanConstraint > cons = new osgbDynamics::CardanConstraint(
            rbA, aXform, rbB, bXform, axisA, axisB );

        if( cons->getAsBtUniversal() == NULL )
            ERROR(testName,"won't typecast as btUniversalConstraint.");

        if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
            ERROR(testName,"writeObjectFile failed.");

        osg::Object* obj = osgDB::readObjectFile( fileName );
        if( obj == NULL )
            ERROR(testName,"readObjectFile returned NULL.");

        osg::ref_ptr< osgbDynamics::CardanConstraint > cons2 = dynamic_cast<
            osgbDynamics::CardanConstraint* >( obj );
        if( !( cons2.valid() ) )
            ERROR(testName,"dynamic_cast after readObjectFile failed.");

        if( *cons2 != *cons )
            // Note matches can fail due to double precision roundoff.
            // For testing, use only 1s and 0s in matrices.
            ERROR(testName,"failed to match.");

        return( 0 );
    }

    //
    // RagdollConstraint
    if( testName == std::string( "Ragdoll" ) )
    {
        osg::Vec3 point( 0., 1., 2. );
        osg::Vec3 axis( 0., 1., 0. );
        double angle = 2.;
        osg::ref_ptr< osgbDynamics::RagdollConstraint > cons = new osgbDynamics::RagdollConstraint(
            rbA, aXform, rbB, bXform, point, axis, angle );

        if( cons->getAsBtConeTwist() == NULL )
            ERROR(testName,"won't typecast as btConeTwistConstraint.");

        if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
            ERROR(testName,"writeObjectFile failed.");

        osg::Object* obj = osgDB::readObjectFile( fileName );
        if( obj == NULL )
            ERROR(testName,"readObjectFile returned NULL.");

        osg::ref_ptr< osgbDynamics::RagdollConstraint > cons2 = dynamic_cast<
            osgbDynamics::RagdollConstraint* >( obj );
        if( !( cons2.valid() ) )
            ERROR(testName,"dynamic_cast after readObjectFile failed.");

        if( *cons2 != *cons )
            // Note matches can fail due to double precision roundoff.
            // For testing, use only 1s and 0s in matrices.
            ERROR(testName,"failed to match.");

        return( 0 );
    }

    //
    // WheelSuspensionConstraint
    if( testName == std::string( "WheelSuspension" ) )
    {
        osg::Vec3 springAxis( 0., 0., 1. );
        osg::Vec3 axleAxis( 0., 1., 0. );
        osg::Vec2 linearLimit( -2., 3. );
        osg::Vec2 angleLimit( -1., 1. );
        osg::Vec3 anchor( 0., 1., 2. );

        osg::ref_ptr< osgbDynamics::WheelSuspensionConstraint > cons = new osgbDynamics::WheelSuspensionConstraint(
            rbA, rbB, springAxis, axleAxis, linearLimit, angleLimit, anchor );

        if( cons->getAsBtHinge2() == NULL )
            ERROR(testName,"won't typecast as btHinge2Constraint.");

        if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
            ERROR(testName,"writeObjectFile failed.");

        osg::Object* obj = osgDB::readObjectFile( fileName );
        if( obj == NULL )
            ERROR(testName,"readObjectFile returned NULL.");

        osg::ref_ptr< osgbDynamics::WheelSuspensionConstraint > cons2 = dynamic_cast<
            osgbDynamics::WheelSuspensionConstraint* >( obj );
        if( !( cons2.valid() ) )
            ERROR(testName,"dynamic_cast after readObjectFile failed.");

        if( *cons2 != *cons )
            // Note matches can fail due to double precision roundoff.
            // For testing, use only 1s and 0s in matrices.
            ERROR(testName,"failed to match.");

        return( 0 );
    }

    ERROR(testName,"unknown test name.");
}
int mitkSliceNavigationControllerTest(int /*argc*/, char* /*argv*/[])
{
   int result=EXIT_FAILURE;

   std::cout << "Creating and initializing a PlaneGeometry: ";
   mitk::PlaneGeometry::Pointer planegeometry = mitk::PlaneGeometry::New();

   mitk::Point3D origin;
   mitk::Vector3D right, bottom, normal;
   mitk::ScalarType width, height;
   mitk::ScalarType widthInMM, heightInMM, thicknessInMM;

   width  = 100;    widthInMM  = width;
   height = 200;    heightInMM = height;
   thicknessInMM = 1.5;
   //  mitk::FillVector3D(origin,         0,          0, thicknessInMM*0.5);
   mitk::FillVector3D(origin, 4.5,              7.3, 11.2);
   mitk::FillVector3D(right,  widthInMM,          0, 0);
   mitk::FillVector3D(bottom,         0, heightInMM, 0);
   mitk::FillVector3D(normal,         0,          0, thicknessInMM);

   mitk::Vector3D spacing;
   normal.Normalize(); normal *= thicknessInMM;
   mitk::FillVector3D(spacing, 1.0, 1.0, thicknessInMM);
   planegeometry->InitializeStandardPlane(right.GetVnlVector(), bottom.GetVnlVector(), &spacing);
   planegeometry->SetOrigin(origin);
   std::cout<<"[PASSED]"<<std::endl;

   std::cout << "Creating and initializing a SlicedGeometry3D with the PlaneGeometry: ";
   mitk::SlicedGeometry3D::Pointer slicedgeometry = mitk::SlicedGeometry3D::New();
   unsigned int numSlices = 5;
   slicedgeometry->InitializeEvenlySpaced(planegeometry, thicknessInMM, numSlices, false);
   std::cout<<"[PASSED]"<<std::endl;

   std::cout << "Creating a Geometry3D with the same extent as the SlicedGeometry3D: ";
   mitk::Geometry3D::Pointer geometry = mitk::Geometry3D::New();
   geometry->SetBounds(slicedgeometry->GetBounds());
   geometry->SetIndexToWorldTransform(slicedgeometry->GetIndexToWorldTransform());
   std::cout<<"[PASSED]"<<std::endl;

   mitk::Point3D cornerpoint0;
   cornerpoint0 = geometry->GetCornerPoint(0);

   result=testGeometry(geometry, width, height, numSlices, widthInMM, heightInMM, thicknessInMM, cornerpoint0, right, bottom, normal);
   if(result!=EXIT_SUCCESS)
      return result;

   mitk::AffineTransform3D::Pointer transform = mitk::AffineTransform3D::New();
   transform->SetMatrix(geometry->GetIndexToWorldTransform()->GetMatrix());
   mitk::BoundingBox::Pointer boundingbox = geometry->CalculateBoundingBoxRelativeToTransform(transform);
   geometry->SetBounds(boundingbox->GetBounds());
   cornerpoint0 = geometry->GetCornerPoint(0);

   result=testGeometry(geometry, width, height, numSlices, widthInMM, heightInMM, thicknessInMM, cornerpoint0, right, bottom, normal);
   if(result!=EXIT_SUCCESS)
      return result;

   std::cout << "Changing the IndexToWorldTransform of the geometry to a rotated version by SetIndexToWorldTransform() (keep cornerpoint0): ";
   transform = mitk::AffineTransform3D::New();
   mitk::AffineTransform3D::MatrixType::InternalMatrixType vnlmatrix;
   vnlmatrix = planegeometry->GetIndexToWorldTransform()->GetMatrix().GetVnlMatrix();
   mitk::VnlVector axis(3);
   mitk::FillVector3D(axis, 1.0, 1.0, 1.0); axis.normalize();
   vnl_quaternion<mitk::ScalarType> rotation(axis, 0.223);
   vnlmatrix = rotation.rotation_matrix_transpose()*vnlmatrix;
   mitk::Matrix3D matrix;
   matrix = vnlmatrix;
   transform->SetMatrix(matrix);
   transform->SetOffset(cornerpoint0.GetVectorFromOrigin());

   right.SetVnlVector( rotation.rotation_matrix_transpose()*right.GetVnlVector() );
   bottom.SetVnlVector(rotation.rotation_matrix_transpose()*bottom.GetVnlVector());
   normal.SetVnlVector(rotation.rotation_matrix_transpose()*normal.GetVnlVector());
   geometry->SetIndexToWorldTransform(transform);
   std::cout<<"[PASSED]"<<std::endl;

   cornerpoint0 = geometry->GetCornerPoint(0);

   result = testGeometry(geometry, width, height, numSlices, widthInMM, heightInMM, thicknessInMM, cornerpoint0, right, bottom, normal);
   if(result!=EXIT_SUCCESS)
      return result;

   //Testing Execute RestorePlanePositionOperation
   result = testRestorePlanePostionOperation();
   if(result!=EXIT_SUCCESS)
      return result;

   // Re-Orient planes not working as it should on windows
   // However, this might be adjusted during a geometry redesign.
   /*
   //Testing  ReorientPlanes
   result = testReorientPlanes();
   if(result!=EXIT_SUCCESS)
   return result;
   */

   std::cout<<"[TEST DONE]"<<std::endl;
   return EXIT_SUCCESS;
}
void CPhysicSphericalJoint::SetInfoRagdoll	(SSphericalLimitInfo _sInfo, CPhysicActor* actorA,  CPhysicActor* actorB)
{
  if (actorA==NULL)
	{
		LOGGER->AddNewLog(ELL_ERROR, "PhysicSphericalJoint:: El primer actor pasado como argumento no puede ser null");
		return;
	}

  NxVec3 pos(_sInfo.m_vAnchor.x, _sInfo.m_vAnchor.y, _sInfo.m_vAnchor.z );
	NxVec3 axis(_sInfo.m_vAxis.x, _sInfo.m_vAxis.y, _sInfo.m_vAxis.z );

	m_pSphericalDesc->setToDefault();

	m_pSphericalDesc->actor[0] = actorA->GetPhXActor();
	if (actorB!=NULL)
	{
		m_pSphericalDesc->actor[1] = actorB->GetPhXActor();	
	}
	else
	{
		m_pSphericalDesc->actor[1] = NULL;	
	}


 
  //LIMITS PELS TWIST!!!!!!! (gir de munyeca)
  if (_sInfo.TwistLimit)
  {
    m_pSphericalDesc->flags |= NX_SJF_TWIST_LIMIT_ENABLED;
    m_pSphericalDesc->twistLimit.low.value = _sInfo.TwistLowValue*NxPi;
    m_pSphericalDesc->twistLimit.low.restitution = _sInfo.TwistLowRestitution;
    m_pSphericalDesc->twistLimit.high.value = _sInfo.TwistHighValue*NxPi;
    m_pSphericalDesc->twistLimit.high.restitution = _sInfo.TwistHighRestitution;

    m_pSphericalDesc->twistLimit.low.hardness = 0.5f;
    m_pSphericalDesc->twistLimit.high.hardness = 0.5f;

  }

  //Es pot push pero al retornar, com mes petit es el valor, menys espai recorre.
  if (_sInfo.SwingLimit)
  {
    m_pSphericalDesc->flags |= NX_SJF_SWING_LIMIT_ENABLED;
    m_pSphericalDesc->swingLimit.value = _sInfo.SwingValue*NxPi;
    m_pSphericalDesc->swingLimit.restitution = _sInfo.SwingRestitution;
    m_pSphericalDesc->swingLimit.hardness = 0.5f;
  }

  //Twist Spring Enabled
  if (_sInfo.TwistSpring)
  {
    m_pSphericalDesc->flags |= NX_SJF_TWIST_SPRING_ENABLED;
    m_pSphericalDesc->twistSpring.damper = _sInfo.TwistSpringDamper;
    m_pSphericalDesc->twistSpring.spring = _sInfo.TwistSpringValue;
  }

  //Swing Spring Enabled
  if (_sInfo.SwingSpring)
  {
    m_pSphericalDesc->flags |= NX_SJF_SWING_SPRING_ENABLED;
    m_pSphericalDesc->swingSpring.damper = _sInfo.SwingSpringDamper;
    m_pSphericalDesc->swingSpring.spring = _sInfo.SwingSpringValue;
  }
  
  //Joint Springs
  if (_sInfo.JointSpring)
  {
    m_pSphericalDesc->flags |= NX_SJF_JOINT_SPRING_ENABLED;
    m_pSphericalDesc->jointSpring.damper = _sInfo.JointSpringDamper;
    m_pSphericalDesc->jointSpring.spring = _sInfo.JointSpringValue;
  }

  //TODO: Passar per parametre
  m_pSphericalDesc->projectionMode = NX_JPM_POINT_MINDIST;
  m_pSphericalDesc->projectionDistance = 0.22f;
  m_pSphericalDesc->solverExtrapolationFactor = 1.1f;
  
  //Projection per errors
  /*m_pSphericalDesc->projectionMode = NX_JPM_POINT_MINDIST;
  m_pSphericalDesc->projectionDistance = 0.15f;*/

  m_pSphericalDesc->setGlobalAnchor(pos);
  m_pSphericalDesc->setGlobalAxis(axis);

}
Example #9
0
int main(int argc, char** argv)
{
    // use an ArgumentParser object to manage the program arguments.
    osg::ArgumentParser arguments(&argc,argv);

    // set up the usage document, in case we need to print out how to use this program.
    arguments.getApplicationUsage()->setApplicationName(arguments.getApplicationName());
    arguments.getApplicationUsage()->setDescription(arguments.getApplicationName()+" example demonstrates the use of ImageStream for rendering movies as textures.");
    arguments.getApplicationUsage()->setCommandLineUsage(arguments.getApplicationName()+" [options] filename ...");
    arguments.getApplicationUsage()->addCommandLineOption("-h or --help","Display this information");
    arguments.getApplicationUsage()->addCommandLineOption("--texture2D","Use Texture2D rather than TextureRectangle.");
    arguments.getApplicationUsage()->addCommandLineOption("--shader","Use shaders to post process the video.");
    arguments.getApplicationUsage()->addCommandLineOption("--interactive","Use camera manipulator to allow movement around movie.");
    arguments.getApplicationUsage()->addCommandLineOption("--flip","Flip the movie so top becomes bottom.");


    // construct the viewer.
    osgViewer::Viewer viewer(arguments);

    if (arguments.argc()<1)
    {
        arguments.getApplicationUsage()->write(std::cout,osg::ApplicationUsage::COMMAND_LINE_OPTION);
        return 1;
    }

    osg::ref_ptr<osg::Group> root = new osg::Group;



    /*    osg::Light* light = new osg::Light();
    light->setPosition(osg::Vec4d(-500.0, 1000.0, 500.0, 1.0));
    light->setDirection(osg::Vec3d(5.0, -10.0, -5.0));
    light->setSpotCutoff(70);
    light->setAmbient(osg::Vec4d(0.05, 0.05, 0.05, 1.0));
    light->setDiffuse(osg::Vec4d(0.5, 0.5, 0.5, 1.0));
    //light->setQuadraticAttenuation(0.001);

    osg::LightSource* lightSource = new osg::LightSource();
    lightSource->setLight(light);

    //osg::Light * attachedlight = lightSource->getLight();

    //attache light to root group
    root->addChild(lightSource);

	//activate light
	osg::StateSet* stateSet = root->getOrCreateStateSet();
    lightSource->setStateSetModes(*stateSet, osg::StateAttribute::ON);

    osg::StateSet* stateset = root->getOrCreateStateSet();
    stateset->setMode(GL_LIGHTING,osg::StateAttribute::ON);
*/
    osg::ref_ptr<osg::Geode> geode = new osg::Geode;



    //OpenCV-AR
    CvCapture* cameraCapture;
    CvCapture* fileCapture;
    //cameraCapture = cvCreateCameraCapture(0);

    fileCapture = cvCreateFileCapture("video/whal.avi");
    cameraCapture = fileCapture;

    if(!cameraCapture) {
		fprintf(stderr,"OpenCV: Create camera capture failed\n");
		return 1;
	}

    //printf("%f\n", cvGetCaptureProperty(cameraCapture, CV_CAP_PROP_FPS));

    //cvSetCaptureProperty(cameraCapture, CV_CAP_PROP_FRAME_WIDTH, 1280);
    //cvSetCaptureProperty(cameraCapture, CV_CAP_PROP_FRAME_HEIGHT, 960);
    //cvSetCaptureProperty(cameraCapture, CV_CAP_PROP_FPS, 15);

	IplImage* frame = cvQueryFrame(cameraCapture);
	IplImage* flipFrame = cvCreateImage(cvGetSize(frame), frame->depth, frame->nChannels);



    //osg::Image* image = osgDB::readImageFile("aclib-large.png");
    osg::Image* image = new osg::Image();
    //image->setPixelBufferObject( new osg::PixelBufferObject(image));

    image->setDataVariance( osg::Object::DYNAMIC );
    iplImageToOsgImage(flipFrame, image);

    //load model
    osg::ref_ptr<osg::PositionAttitudeTransform> modelPat = new osg::PositionAttitudeTransform();

    //osg::ref_ptr<osg::Node> loadedModel = osgDB::readNodeFile("models/Cars/AstonMartin-DB9.3ds");
    osg::ref_ptr<osg::Node> loadedModel = osgDB::readNodeFile("models/ferrari_car_2.osg");
    modelPat->addChild(loadedModel);
    modelPat->setScale(osg::Vec3(0.5, 0.5, 0.5));
    modelPat->setAttitude(osg::Quat(3.14 / 2, osg::Vec3d(-1.0, 0.0, 0.0)));

    if (!loadedModel) {
    	std::cout << "No model data loaded" << std::endl;
    	return 1;
    }



    //C_BODY

    std::vector<osg::MatrixTransform*> topMtList = getMatrixTransformListByName("C_TOP", loadedModel);
    std::vector<osg::MatrixTransform*> leftDoorMtList = getMatrixTransformListByName("C_LDOOR", loadedModel);
    std::vector<osg::MatrixTransform*> rightDoorMtList = getMatrixTransformListByName("C_RDOOR", loadedModel);
    std::vector<osg::MatrixTransform*> leftWheelsMtList = getMatrixTransformListByName("C_LWS", loadedModel);
    std::vector<osg::MatrixTransform*> rightWheelsMtList = getMatrixTransformListByName("C_RWS", loadedModel);
    std::vector<osg::MatrixTransform*> forwardBumperMtList = getMatrixTransformListByName("C_BUMP_F", loadedModel);
    std::vector<osg::MatrixTransform*> backBumperMtList = getMatrixTransformListByName("C_BUMP_B", loadedModel);
    std::vector<osg::MatrixTransform*> engineMtList = getMatrixTransformListByName("C_ENGINE", loadedModel);
    std::vector<osg::MatrixTransform*> bodyMtList = getMatrixTransformListByName("C_BODY", loadedModel);
    std::vector<osg::MatrixTransform*> salonMtList = getMatrixTransformListByName("C_SALON", loadedModel);

    /*    //findNodeVisitor findNode("C_BODY");
    FindNamedNode findNode("C_BODY");
    loadedModel->accept(findNode);

    std::vector<osg::Node*> foundNodeList = findNode.getNodeList();
    int listCount = foundNodeList.size();
    printf("%d\n", listCount);
    std::vector<osg::MatrixTransform*> bodyMtList;

    //vector<int>::const_iterator i;
    for(int i = 0; i < listCount; i++) {
        bodyMtList.push_back(new osg::MatrixTransform());
        //obj4Mt->setName("obj4Mt");

        osg::Group* foundNodeParent = foundNodeList[i]->getParent(0);

        bodyMtList[i]->addChild(foundNodeList[i]);

        foundNodeParent->addChild(bodyMtList[i]);

        foundNodeParent->removeChild(foundNodeList[i]);
    }
*/
    osg::Matrix translateMatrix;


    //osg::Node* foundNode = NULL;
    //foundNode = findNamedNode("obj5", loadedModel);

    //osg::ref_ptr<osg::MatrixTransform> obj5Mt = new osg::MatrixTransform();
    //obj4Mt->setName("obj5Mt");

    //osg::Group* foundNodeParent = foundNode->getParent(0);

    //obj5Mt->addChild(foundNode);

    //foundNodeParent->addChild(obj5Mt);

    //foundNodeParent->removeChild(foundNode);


    osg::Matrix rotateMatrix;
    float theta(M_PI * 0.1f);
    osg::Vec3f axis (1.0, 1.0, 0.1);
    osg::Quat wheelAxis( theta, axis);



    osg::BoundingSphere modelBoundingSphere = modelPat->getBound();
    printf("%f\n", modelBoundingSphere.radius());
    modelBoundingSphere.radius() *= 1.5f;

    osg::BoundingBox modelBoundingBox;
    modelBoundingBox.expandBy(modelBoundingSphere);



    //Light group

    //create light
    root->addChild(createLights(modelBoundingBox, root->getOrCreateStateSet()));


    //collect scene

    // only clear the depth buffer
    viewer.getCamera()->setClearMask(GL_DEPTH_BUFFER_BIT);

    // create a HUD as slave camera attached to the master view.

    viewer.setUpViewAcrossAllScreens();

    osgViewer::Viewer::Windows windows;
    viewer.getWindows(windows);

    if (windows.empty()) return 1;

    osg::Camera* screenCamera = createScreen(image);

    // set up cameras to rendering on the first window available.
    screenCamera->setGraphicsContext(windows[0]);
    screenCamera->setViewport(0,0,windows[0]->getTraits()->width, windows[0]->getTraits()->height);
    //screenCamera->setViewport(0, 0, 6.4, 4.8);


    viewer.addSlave(screenCamera, false);


    //root->addChild( geode.get());
    //root->addChild( createPyramid());
    //root->addChild( createScreen());//100.0, 100.0, image));
    root->addChild(modelPat);
    //root->addChild(objectPat);

    // set the scene to render
    viewer.setSceneData(root.get());



        viewer.realize();
        
        viewer.getCamera()->setClearColor(osg::Vec4(0.0f,0.0f,0.0f,1.0f));
/*
        //viewer.getCamera()->setProjameraCaptureectionMatrixAsOrtho(topleft.x(),bottomright.x(),topleft.y(),bottomright.y(), -10, 10);
        //viewer.getCamera()->setProjectionMatrixAsPerspective(60.0, screenAspectRatio, 100.0, -1.0);
*/
        viewer.getCamera()->setViewMatrixAsLookAt(osg::Vec3d(100.0, 100.0, 100.0), osg::Vec3d(0.0, 0.0, 0.0), osg::Vec3d(0.0, 1.0, 0.0));






        //Define vector of OpenCV-AR template
        vector<CvarTemplate> openCvArTemplateList;


        //load template
        CvarTemplate openCvArTemplate1;
        cvarLoadTemplateTag(&openCvArTemplate1,"aclib.png");
        //cvarLoadTemplateTag(&openCvArTemplate1,"markers/431.jpg");
        openCvArTemplateList.push_back(openCvArTemplate1);

        CvarTemplate openCvArTemplate2;
        cvarLoadTemplate(&openCvArTemplate2,"aclib.png",1);
        //cvarLoadTemplate(&openCvArTemplate2,"markers/431.jpg", 1);
        openCvArTemplateList.push_back(openCvArTemplate2);

        //Define OpenCV-AR marker;
        vector<CvarMarker> openCvArMarker;

        //Create OpenCV-AR camera
        CvarCamera openCvArCamera;
        //IplImage* frame = osgImage2IplImage(image);

        //cvarReadCamera("camera.yml", &openCvArCamera);
        cvarReadCamera(NULL, &openCvArCamera);
        cvarCameraScale(&openCvArCamera,frame->width,frame->height);
        viewer.getCamera()->setProjectionMatrix(osg::Matrixd(openCvArCamera.projection));


        //CvarOpticalFlow *flow;

       // srand(time(NULL));

        //int thresh = 60;
        double matchThresh = 0.7;
        //int state = 0;


        int counter = 0;
        while(!viewer.done())
        {
        	counter++;

        	char c = 0;//cvWaitKey(33);
        	//printf("%d\n", c);
        	if (c == 27) { // нажата ESC
        		printf("esc\n");
        		break;
        	}
        	if (c == 107) { // matchThresh up
        	  	matchThresh = matchThresh + 0.01;
        	}
        	if (c == 106) { // matchThresh down
        		matchThresh = matchThresh - 0.01;
        	}


        	if ((counter >= 300) and (counter < 310)) { // matchThresh down
        		//Top
        		translateMatrixTransformList(topMtList, 0.0, -1.2, 0.0);

        		//Engine
        		translateMatrixTransformList(engineMtList, 0.0, -1.0, 0.0);

        		//Body
        		translateMatrixTransformList(bodyMtList, 0.0, -0.8, 0.0);

        		//Salon
        		translateMatrixTransformList(salonMtList, 0.0, -0.4, 0.0);


        		//leftWeels
        		translateMatrixTransformList(leftWheelsMtList, -0.5, 0.0, 0.0);

        		//rightWeels
        		translateMatrixTransformList(rightWheelsMtList, 0.5, 0.0, 0.0);

        		//Left doors
        		translateMatrixTransformList(leftDoorMtList, -0.5, 0.0, 0.0);

        		//Right doors
           		translateMatrixTransformList(rightDoorMtList, 0.5, 0.0, 0.0);


           		//Forward bumper
        		translateMatrixTransformList(forwardBumperMtList, 0.0, 0.0, 0.5);

        		//back bumper
        		translateMatrixTransformList(backBumperMtList, 0.0, 0.0, -0.5);


        	}


        	//rotateMatrix.makeRotate(rotateMatrix.getRotate() * wheelAxis);
        	//obj5Mt->setMatrix(rotateMatrix);

        	//thresh = rand() % 256;
        	//printf("Match thresh value: %f\n", matchThresh);
        	frame = cvQueryFrame(cameraCapture);

        	cvCopy(frame, flipFrame);

        	cvFlip(flipFrame, flipFrame);
        	//cvNamedWindow("Original", 1);
        	//cvShowImage("Original", frame);
        	iplImageToOsgImage(frame, image);

        	image->dirty();

        	//osg::Image* = osg::Image(*image);


        	//frame = osgImage2IplImage(image);
        	//AR detection
        	//GLdouble modelview[16] = {0};

        	//Detect marker
        	int arDetect = cvarArMultRegistration(flipFrame,&openCvArMarker,openCvArTemplateList,&openCvArCamera, 60, 0.91);
			//printf("Marker found: %d\n",  arDetect);

			viewer.getCamera()->setViewMatrixAsLookAt(osg::Vec3d(0.0, 0.0, 100.0), osg::Vec3d(0.0, 0.0, 1000.0), osg::Vec3d(0.0, 1.0, 0.0));

			for(int i=0;i<arDetect;i++) {
				//if(openCvArMarker[i].tpl == 0);
				osg::Matrixf osgModelViewMatrix;
				for (int column = 0; column < 4; column++)	{
					for (int row = 0; row < 4; row++)	{
						osgModelViewMatrix(column, row) = openCvArMarker[i].modelview[column * 4 + row];
					}
				}

				viewer.getCamera()->setViewMatrix(osgModelViewMatrix);
			}

			viewer.frame();
        }
        return 0;

}
Example #10
0
// A "wibbly" is a sphere-man with his center of mass brought low inside the body so he wobbles easily.
// Note that we do not share shapes. We could share the spheres and boxes used since they're of fixed size
// but we create new ones each when time using this method.
hkpRigidBody* CompoundBodiesDemo::createWibbly(hkVector4& position, hkPseudoRandomGenerator* generator)
{
	// We build a wibbly from 4 bodies:
	// 1. A big sphere
	// 2,3 Two arms
	// 4 A head

		// These parameters specify the wibbly size. The main (body) sphere has the radius defined
		// below. The arms have size 'boxSize'.
	hkReal radius = 1.0f;
	hkVector4 boxSize( 1.0f, 0.5f, 0.5f);
	hkReal mass = 10.0f;


	//
	// Create the shapes (we could share these between wibblies of the same size, but we don't here)
	//

	hkpSphereShape* sphere = new hkpSphereShape(radius);

	hkpSphereShape* sphere2 = new hkpSphereShape(radius * 0.3f);

	hkVector4 halfExtents(boxSize(0) * 0.5f, boxSize(1) * 0.5f, boxSize(2) * 0.5f);

	hkpBoxShape* cube = new hkpBoxShape(halfExtents, 0 );	

	//
	// Create a rigid body construction template and start filling it out
	//
	hkpRigidBodyCinfo compoundInfo;


	// We'll basically have to create a 'List' Shape  (ie. a hkpListShape) in order to have many
	// shapes in the same body. Each element of the list will be a (transformed) hkpShape, ie.
	// a hkpTransformShape (which basically is a (geometry,transformation) pair).
	// The hkpListShape constructor needs a pointer to an array of hkShapes, so we create an array here, and push
	// back the hkTransformShapes as we create them.
	hkInplaceArray<hkpShape*,4> shapeArray;

	// Create body
	{
		sphere->addReference();
		shapeArray.pushBack(sphere);
	}

	// Create head
	{
		hkVector4 offset(0.0f, 1.1f, 0.0f);
		hkpConvexTranslateShape* sphere2Trans = new hkpConvexTranslateShape( sphere2, offset );
		shapeArray.pushBack(sphere2Trans);
	}


	// Create right arm
	{

		hkTransform t;
		hkVector4 v(0.0f,0.0f,1.0f);
		hkQuaternion r(v, 0.4f);
		t.setRotation(r);
		t.getTranslation().set(0.9f, .7f, 0.0f);
		hkpConvexTransformShape* cubeTrans = new hkpConvexTransformShape( cube,t );
		shapeArray.pushBack(cubeTrans);
	}


	// Create left arm
	{
		hkTransform t;
		hkVector4 v(0.0f,0.0f,1.0f);
		hkQuaternion r(v, -0.4f);
		t.setRotation(r);
		t.getTranslation().set(-0.9f, .7f, 0.0f);
		hkpConvexTransformShape* cubeTrans = new hkpConvexTransformShape( cube,t );
		shapeArray.pushBack(cubeTrans);
	}



	// Now we can create the compound body as a hkpListShape

	hkpListShape* listShape;
	{
		listShape = new hkpListShape(shapeArray.begin(), shapeArray.getSize());
		sphere->removeReference();
		sphere2->removeReference();
		cube->removeReference();
		for (int i = 0; i < shapeArray.getSize(); ++i)
		{
			shapeArray[i]->removeReference();
		}
	}
	compoundInfo.m_shape = listShape;


	//
	// Create the rigid body
	//

	compoundInfo.m_mass = mass;
	
		// Fake an inertia tensor using a cube of side 'radius'
	hkVector4 halfCube(radius *0.5f, radius *0.5f, radius *0.5f);
	hkpMassProperties massProperties;
	hkpInertiaTensorComputer::computeBoxVolumeMassProperties(halfCube, mass, massProperties);
	compoundInfo.m_inertiaTensor = massProperties.m_inertiaTensor;

	// Now (and here's the wibbly bit) set the center of mass to be near the bottom of the
	// body sphere.
	compoundInfo.m_centerOfMass.set(0.0f,-0.8f,0.0f);
	compoundInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;

	compoundInfo.m_position = position;
		// Use "random" initial orientation
	hkVector4 axis(generator->getRandRange(-1.0f,1.0f),generator->getRandRange(-1.0f,1.0f),generator->getRandRange(-1.0f,1.0f));
	axis.normalize3();
	hkQuaternion q(axis, generator->getRandRange(0,HK_REAL_PI));
	compoundInfo.m_rotation = q;


		// Finally create the body
	hkpRigidBody* compoundRigidBody = new hkpRigidBody(compoundInfo);

	listShape->removeReference();

	return compoundRigidBody;

}
Example #11
0
bool ChunkyBoneGeometry::CreateJoint(ChunkyPhysics* structure, PhysicsManager* physics, unsigned physics_fps) {
	bool ok = false;
	if (body_data_.parent_) {
		if (GetBoneType() == kBonePosition) {
			// Need not do jack. It's not a physical object.
			ok = true;
		} else if (body_data_.joint_type_ == kJointExclude) {
			ok = physics->Attach(GetBodyId(), body_data_.parent_->GetBodyId());
		} else if (body_data_.joint_type_ == kJointFixed) {
			ok = physics->Attach(GetBodyId(), body_data_.parent_->GetBodyId());
		} else if (body_data_.joint_type_ == kJointSuspendHinge || body_data_.joint_type_ == kJointHinge2) {
			// Calculate axis from given euler angles.
			vec3 suspension_axis(-1, 0, 0);
			vec3 hinge_axis(0, 0, 1);
			quat rotator;
			rotator.SetEulerAngles(body_data_.parameter_[kParamEulerTheta], 0, body_data_.parameter_[kParamEulerPhi]);
			suspension_axis = rotator*suspension_axis;
			hinge_axis = rotator*hinge_axis;

			joint_id_ = physics->CreateHinge2Joint(body_data_.parent_->GetBodyId(),
				GetBodyId(), structure->GetTransformation(this).GetPosition(),
				suspension_axis, hinge_axis);
			physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
			physics->SetSuspension(joint_id_, 1/(float)physics_fps, body_data_.parameter_[kParamSpringConstant],
				body_data_.parameter_[kParamSpringDamping]);
			physics->SetAngularMotorRoll(joint_id_, 0, 0);
			physics->SetAngularMotorTurn(joint_id_, 0, 0);
			ok = true;
		} else if (body_data_.joint_type_ == kJointHinge) {
			// Calculate axis from given euler angles.
			vec3 hinge_axis(0, 0, 1);
			quat hinge_rotator;
			hinge_rotator.SetEulerAngles(body_data_.parameter_[kParamEulerTheta], 0, body_data_.parameter_[kParamEulerPhi]);
			hinge_axis = hinge_rotator*hinge_axis;

			const xform& body_transform = structure->GetTransformation(this);
			const vec3 anchor = body_transform.GetPosition() + GetOriginalOffset();
			joint_id_ = physics->CreateHingeJoint(body_data_.parent_->GetBodyId(),
				GetBodyId(), anchor, hinge_axis);
			physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], body_data_.bounce_);
			physics->SetAngularMotorTurn(joint_id_, 0, 0);
			//physics->GetAxis1(joint_id_, hinge_axis);
			ok = true;
		} else if (body_data_.joint_type_ == kJointSlider) {
			// Calculate axis from given euler angles.
			vec3 axis(0, 0, 1);
			quat rotator;
			rotator.SetEulerAngles(body_data_.parameter_[kParamEulerTheta], 0, body_data_.parameter_[kParamEulerPhi]);
			axis = rotator*axis;

			joint_id_ = physics->CreateSliderJoint(body_data_.parent_->GetBodyId(),
				GetBodyId(), axis);
			physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], body_data_.bounce_);
			physics->SetMotorTarget(joint_id_, 0, 0);
			ok = true;
		} else if (body_data_.joint_type_ == kJointUniversal) {
			// Calculate axis from given euler angles.
			vec3 axis1(0, 0, 1);
			vec3 axis2(0, 1, 0);
			quat rotator;
			rotator.SetEulerAngles(body_data_.parameter_[kParamEulerTheta], 0, body_data_.parameter_[kParamEulerPhi]);
			axis1 = rotator*axis1;
			axis2 = rotator*axis2;

			const xform& body_transform = structure->GetTransformation(this);
			const vec3 anchor = body_transform.GetPosition() +
				vec3(body_data_.parameter_[kParamOffsetX], body_data_.parameter_[kParamOffsetY], body_data_.parameter_[kParamOffsetZ]);
			joint_id_ = physics->CreateUniversalJoint(body_data_.parent_->GetBodyId(),
				GetBodyId(), anchor, axis1, axis2);
			physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
			/*physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
			physics->SetSuspension(joint_id_, 1/(float)physics_fps, body_data_.parameter_[0],
				body_data_.parameter_[1]);
			physics->SetAngularMotorRoll(joint_id_, 0, 0);
			physics->SetAngularMotorTurn(joint_id_, 0, 0);*/
			ok = true;
		} else if (body_data_.joint_type_ == kJointBall) {
			const xform& body_transform = structure->GetTransformation(this);
			const vec3 anchor = body_transform.GetPosition() +
				vec3(body_data_.parameter_[kParamOffsetX], body_data_.parameter_[kParamOffsetY], body_data_.parameter_[kParamOffsetZ]);
			joint_id_ = physics->CreateBallJoint(body_data_.parent_->GetBodyId(),
				GetBodyId(), anchor);
			/*physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
			physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
			physics->SetSuspension(joint_id_, 1/(float)physics_fps, body_data_.parameter_[0],
				body_data_.parameter_[1]);
			physics->SetAngularMotorRoll(joint_id_, 0, 0);
			physics->SetAngularMotorTurn(joint_id_, 0, 0);*/
			ok = true;
		} else {
			deb_assert(false);
		}
	} else {
		deb_assert(body_data_.joint_type_ == kJointExclude);
		ok = true;
	}
	deb_assert(ok);
	return (ok);
}
CollisionEventsDemo::CollisionEventsDemo( hkDemoEnvironment* env) 
	: hkDefaultPhysicsDemo(env) 
{
	//
	// Setup the camera
	//
	{
		hkVector4 from(3.0f, 4.0f, 8.0f);
		hkVector4 to(0.0f, 1.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.setBroadPhaseWorldSize( 100.0f );
		info.m_simulationType = info.SIMULATION_TYPE_CONTINUOUS;

		// Turn off deactivation so we can see continuous contact point processing
		info.m_enableDeactivation = false;
		m_world = new hkpWorld( info );
		m_world->lock();

		setupGraphics();
	}

	//
	// Register the box-box collision agent
	//
	{
		hkpAgentRegisterUtil::registerAllAgents(m_world->getCollisionDispatcher());
	}




	//
	// Create the floor
	//
	{
		hkpRigidBodyCinfo info;
		hkVector4 fixedBoxSize(5.0f, 0.5f , 5.0f );
		hkpBoxShape* fixedBoxShape = new hkpBoxShape( fixedBoxSize , 0 );

		info.m_shape = fixedBoxShape;
		info.m_motionType = hkpMotion::MOTION_FIXED;
		info.m_position.setZero4();

		// Add some bounce.
		info.m_restitution = 0.8f;
		info.m_friction = 1.0f;

		// Create fixed box
		hkpRigidBody* floor = new hkpRigidBody(info);
		m_world->addEntity(floor);

		floor->removeReference();
		fixedBoxShape->removeReference();
	}

	// For this demo we simply have two box shapes which are constructed in the usual manner using a hkpRigidBodyCinfo 'blueprint'.
	// The dynamic box creation code in presented below. There are two key things to note in this example;
	// the 'm_restitution' member variable, which we have explicitly set to value of 0.9.
	// The restitution is over twice the default value of
	// 0.4 and is set to give the box (the floor has been set likewise) a more 'bouncy' nature. 

	//
	// Create a moving box
	//
	{
		hkpRigidBodyCinfo boxInfo;
		hkVector4 boxSize( .5f, .5f ,.5f );
		boxInfo.m_shape = new hkpBoxShape( boxSize , 0 );


		// Compute the box inertia tensor
		hkpInertiaTensorComputer::setShapeVolumeMassProperties( boxInfo.m_shape, 1.0f, boxInfo );

		boxInfo.m_qualityType = HK_COLLIDABLE_QUALITY_DEBRIS;
		boxInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;

		// Place the box so it bounces interestingly
		boxInfo.m_position.set(0.0f, 5.0f, 0.0f);
		hkVector4 axis(1.0f, 2.0f, 3.0f);
		axis.normalize3();
		boxInfo.m_rotation.setAxisAngle(axis, -0.7f);

		// Add some bounce.
		boxInfo.m_restitution = 0.5f;
		boxInfo.m_friction = 0.1f;

		hkpRigidBody* boxRigidBody = new hkpRigidBody( boxInfo );

		// remove reference from boxShape since rigid body "owns" it
		boxInfo.m_shape->removeReference();

		m_world->addEntity( boxRigidBody );
		boxRigidBody->removeReference();
		
		// Add the collision event listener to the rigid body
		MyCollisionListener* listener = new MyCollisionListener( boxRigidBody );
		listener->m_reportLevel = m_env->m_reportingLevel;
	}

	m_world->unlock();

}
// input: cloudInput
// input: pointCloudNormals
// output: cloudOutput
// output: pointCloudNormalsFiltered
void extractHandles(PointCloud::Ptr& cloudInput, PointCloud::Ptr& cloudOutput, PointCloudNormal::Ptr& pointCloudNormals, std::vector<int>& handles) {
	// PCL objects
	//pcl::PassThrough<Point> vgrid_;                   // Filtering + downsampling object
	pcl::VoxelGrid<Point> vgrid_; // Filtering + downsampling object
	pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_; // Planar segmentation object
	pcl::SACSegmentation<Point> seg_line_; // Planar segmentation object
	pcl::ProjectInliers<Point> proj_; // Inlier projection object
	pcl::ExtractIndices<Point> extract_; // Extract (too) big tables
	pcl::ConvexHull<Point> chull_;
	pcl::ExtractPolygonalPrismData<Point> prism_;
	pcl::PointCloud<Point> cloud_objects_;
	pcl::EuclideanClusterExtraction<Point> cluster_, handle_cluster_;
	pcl::PCDWriter writer;

	double sac_distance_, normal_distance_weight_;
	double eps_angle_, seg_prob_;
	int max_iter_;

	sac_distance_ = 0.05;  //0.02
	normal_distance_weight_ = 0.05;
	max_iter_ = 500;
	eps_angle_ = 30.0; //20.0
	seg_prob_ = 0.99;
	btVector3 axis(0.0, 0.0, 1.0);

	seg_.setModelType(pcl::SACMODEL_NORMAL_PARALLEL_PLANE);
	seg_.setMethodType(pcl::SAC_RANSAC);
	seg_.setDistanceThreshold(sac_distance_);
	seg_.setNormalDistanceWeight(normal_distance_weight_);
	seg_.setOptimizeCoefficients(true);
	seg_.setAxis(Eigen::Vector3f(fabs(axis.getX()), fabs(axis.getY()), fabs(axis.getZ())));
	seg_.setEpsAngle(pcl::deg2rad(eps_angle_));
	seg_.setMaxIterations(max_iter_);
	seg_.setProbability(seg_prob_);

	cluster_.setClusterTolerance(0.03);
	cluster_.setMinClusterSize(200);
	KdTreePtr clusters_tree_(new KdTree);
	clusters_tree_->setEpsilon(1);
	cluster_.setSearchMethod(clusters_tree_);

	seg_line_.setModelType(pcl::SACMODEL_LINE);
	seg_line_.setMethodType(pcl::SAC_RANSAC);
	seg_line_.setDistanceThreshold(0.05);
	seg_line_.setOptimizeCoefficients(true);
	seg_line_.setMaxIterations(max_iter_);
	seg_line_.setProbability(seg_prob_);

	//filter cloud
	double leafSize = 0.001;
	pcl::VoxelGrid<Point> sor;
	sor.setInputCloud (cloudInput);
	sor.setLeafSize (leafSize, leafSize, leafSize);
	sor.filter (*cloudOutput);
	//sor.setInputCloud (pointCloudNormals);
	//sor.filter (*pointCloudNormalsFiltered);
	//std::vector<int> tempIndices;
	//pcl::removeNaNFromPointCloud(*cloudInput, *cloudOutput, tempIndices);
	//pcl::removeNaNFromPointCloud(*pointCloudNormals, *pointCloudNormalsFiltered, tempIndices);

	// Segment planes
	pcl::OrganizedMultiPlaneSegmentation<Point, PointNormal, pcl::Label> mps;
	ROS_INFO("Segmenting planes");
	mps.setMinInliers (20000);
	mps.setMaximumCurvature(0.02);
	mps.setInputNormals (pointCloudNormals);
	mps.setInputCloud (cloudInput);
	std::vector<pcl::PlanarRegion<Point> > regions;
	std::vector<pcl::PointIndices> regionPoints;
	std::vector< pcl::ModelCoefficients > planes_coeff;
	mps.segment(planes_coeff, regionPoints);
	ROS_INFO_STREAM("Number of regions:" << regionPoints.size());

	if ((int) regionPoints.size() < 1) {
		ROS_ERROR("no planes found");
		return;
	}

  	std::stringstream filename;
	for (size_t plane = 0; plane < regionPoints.size (); plane++)
	{
		filename.str("");
		filename << "plane" << plane << ".pcd";
		writer.write(filename.str(), *cloudInput, regionPoints[plane].indices, true);
		ROS_INFO("Plane model: [%f, %f, %f, %f] with %d inliers.",
				planes_coeff[plane].values[0], planes_coeff[plane].values[1],
				planes_coeff[plane].values[2], planes_coeff[plane].values[3], (int)regionPoints[plane].indices.size ());

		//Project Points into the Perfect plane
		PointCloud::Ptr cloud_projected(new PointCloud());
		pcl::PointIndicesPtr cloudPlaneIndicesPtr(new pcl::PointIndices(regionPoints[plane]));
		pcl::ModelCoefficientsPtr coeff(new pcl::ModelCoefficients(planes_coeff[plane]));
		proj_.setInputCloud(cloudInput);
		proj_.setIndices(cloudPlaneIndicesPtr);
		proj_.setModelCoefficients(coeff);
		proj_.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
		proj_.filter(*cloud_projected);

		PointCloud::Ptr cloud_hull(new PointCloud());
		// Create a Convex Hull representation of the projected inliers
		chull_.setInputCloud(cloud_projected);
		chull_.reconstruct(*cloud_hull);
		ROS_INFO("Convex hull has: %d data points.", (int)cloud_hull->points.size ());
		if ((int) cloud_hull->points.size() == 0)
		{
			ROS_WARN("Convex hull has: %d data points. Returning.", (int)cloud_hull->points.size ());
			return;
		}

		// Extract the handle clusters using a polygonal prism
		pcl::PointIndices::Ptr handlesIndicesPtr(new pcl::PointIndices());
		prism_.setHeightLimits(0.05, 0.1);
		prism_.setInputCloud(cloudInput);
		prism_.setInputPlanarHull(cloud_hull);
		prism_.segment(*handlesIndicesPtr);

		ROS_INFO("Number of handle candidates: %d.", (int)handlesIndicesPtr->indices.size ());
		if((int)handlesIndicesPtr->indices.size () < 1100)
			continue;

		/*######### handle clustering code
		handle_clusters.clear();
		handle_cluster_.setClusterTolerance(0.03);
		handle_cluster_.setMinClusterSize(200);
		handle_cluster_.setSearchMethod(clusters_tree_);
		handle_cluster_.setInputCloud(handles);
		handle_cluster_.setIndices(handlesIndicesPtr);
		handle_cluster_.extract(handle_clusters);
		for(size_t j = 0; j < handle_clusters.size(); j++)
		{
			filename.str("");
			filename << "handle" << (int)i << "-" << (int)j << ".pcd";
			writer.write(filename.str(), *handles, handle_clusters[j].indices, true);
		}*/

		pcl::StatisticalOutlierRemoval<Point> sor;
		PointCloud::Ptr cloud_filtered (new pcl::PointCloud<Point>);
		sor.setInputCloud (cloudInput);
		sor.setIndices(handlesIndicesPtr);
		sor.setMeanK (50);
		sor.setStddevMulThresh (1.0);
		sor.filter (*cloud_filtered);
		PointCloudNormal::Ptr cloud_filtered_hack (new PointCloudNormal);
		pcl::copyPointCloud(*cloud_filtered, *cloud_filtered_hack);

		// Our handle is in cloud_filtered.  We want indices of a cloud (also filtered for NaNs)
		pcl::KdTreeFLANN<PointNormal> kdtreeNN;
		std::vector<int> pointIdxNKNSearch(1);
		std::vector<float> pointNKNSquaredDistance(1);
		kdtreeNN.setInputCloud(pointCloudNormals);
		for(size_t j = 0; j < cloud_filtered_hack->points.size(); j++)
		{
			kdtreeNN.nearestKSearch(cloud_filtered_hack->points[j], 1, pointIdxNKNSearch, pointNKNSquaredDistance);
			handles.push_back(pointIdxNKNSearch[0]);
		}
	}
}
Example #14
0
void ScaleDraw::drawBackbone(QPainter *painter) const
{
    ScaleEngine *sc_engine = static_cast<ScaleEngine *>(d_plot->axisScaleEngine(axis()));
    /*QwtScaleEngine *qwtsc_engine=d_plot->axisScaleEngine(axis());
    ScaleEngine *sc_engine =dynamic_cast<ScaleEngine*>(qwtsc_engine);
    if(sc_engine!=NULL)
    {*/
    if (!sc_engine->hasBreak()) {
        const int len = length();
        const int bw = painter->pen().width();
        const int bw2 = bw / 2;
        QPoint pos = this->pos();
        switch(alignment()) {
        case LeftScale:
            QwtPainter::drawLine(painter, pos.x() - bw2,
                                 pos.y(), pos.x() - bw2, pos.y() + len );
            break;
        case RightScale:
            QwtPainter::drawLine(painter, pos.x() + bw2,
                                 pos.y(), pos.x() + bw2, pos.y() + len);
            break;
        case TopScale:
            QwtPainter::drawLine(painter, pos.x(), pos.y() - bw2,
                                 pos.x() + len, pos.y() - bw2);
            break;
        case BottomScale:
            QwtPainter::drawLine(painter, pos.x(), pos.y() + bw2,
                                 pos.x() + len, pos.y() + bw2);
            break;
        }
        return;
    }

    QwtScaleMap scaleMap = map();
    const QwtMetricsMap metricsMap = QwtPainter::metricsMap();
    QPoint pos = this->pos();

    if ( !metricsMap.isIdentity() ) {
        QwtPainter::resetMetricsMap();
        pos = metricsMap.layoutToDevice(pos);

        if ( orientation() == Qt::Vertical ) {
            scaleMap.setPaintInterval(
                metricsMap.layoutToDeviceY((int)scaleMap.p1()),
                metricsMap.layoutToDeviceY((int)scaleMap.p2()));
        } else {
            scaleMap.setPaintInterval(
                metricsMap.layoutToDeviceX((int)scaleMap.p1()),
                metricsMap.layoutToDeviceX((int)scaleMap.p2()));
        }
    }

    const int start = scaleMap.transform(sc_engine->axisBreakLeft());
    const int end = scaleMap.transform(sc_engine->axisBreakRight());
    int lb = start, rb = end;
    if (sc_engine->testAttribute(QwtScaleEngine::Inverted)) {
        lb = end;
        rb = start;
    }

    const int bw = painter->pen().width();
    const int bw2 = bw / 2;
    const int len = length() - 1;
    int aux;
    switch(alignment())
    {
    case LeftScale:
        aux = pos.x() - bw2;
        QwtPainter::drawLine(painter, aux, pos.y(), aux, rb);
        QwtPainter::drawLine(painter, aux, lb + bw, aux, pos.y() + len);
        break;
    case RightScale:
        aux = pos.x() + bw2;
        QwtPainter::drawLine(painter, aux, pos.y(), aux, rb - bw - 1);
        QwtPainter::drawLine(painter, aux, lb - bw2, aux, pos.y() + len);
        break;
    case TopScale:
        aux = pos.y() - bw2;
        QwtPainter::drawLine(painter, pos.x(), aux, lb - bw2, aux);
        QwtPainter::drawLine(painter, rb + bw, aux, pos.x() + len, aux);
        break;
    case BottomScale:
        aux = pos.y() + bw2;
        QwtPainter::drawLine(painter, pos.x(), aux, lb - bw, aux);
        QwtPainter::drawLine(painter, rb, aux, pos.x() + len, aux);
        break;
    }
    //}
}
void HavokExport::makeHavokRigidBody(NiNodeRef parent, INode *ragdollParent, float scale) {

	this->scale = scale;

	Object *Obj = ragdollParent->GetObjectRef();

	Modifier* rbMod = nullptr;
	Modifier* shapeMod = nullptr;
	Modifier* constraintMod = nullptr;

	SimpleObject* havokTaperCapsule = nullptr;

	//get modifiers
	

	while (Obj->SuperClassID() == GEN_DERIVOB_CLASS_ID) {
		IDerivedObject *DerObj = static_cast<IDerivedObject *> (Obj);
		const int nMods = DerObj->NumModifiers(); //it is really the last modifier on the stack, and not the total number of modifiers

		for (int i = 0; i < nMods; i++)
		{
			Modifier *Mod = DerObj->GetModifier(i);
			if (Mod->ClassID() == HK_RIGIDBODY_MODIFIER_CLASS_ID) {
				rbMod = Mod;
			}
			if (Mod->ClassID() == HK_SHAPE_MODIFIER_CLASS_ID) {
				shapeMod = Mod;
			}
			if (Mod->ClassID() == HK_CONSTRAINT_RAGDOLL_CLASS_ID || Mod->ClassID() == HK_CONSTRAINT_HINGE_CLASS_ID) {
				constraintMod = Mod;
			}
		}
		if (Obj->SuperClassID() == GEOMOBJECT_CLASS_ID) {
			havokTaperCapsule = (SimpleObject*)Obj;
		}
		Obj = DerObj->GetObjRef();
	}

	
	if (!rbMod) {
		throw exception(FormatText("No havok rigid body modifier found on %s", ragdollParent->GetName()));
	}
	if (!shapeMod) {
		throw exception(FormatText("No havok shape modifier found on %s", ragdollParent->GetName()));
	}

//	Object* taper = ragdollParent->GetObjectRef();
	IParamBlock2* taperParameters = Obj->GetParamBlockByID(PB_TAPEREDCAPSULE_OBJ_PBLOCK);
	float radius;
	enum
	{
		// GENERAL PROPERTIES ROLLOUT
		PA_TAPEREDCAPSULE_OBJ_RADIUS = 0,
		PA_TAPEREDCAPSULE_OBJ_TAPER,
		PA_TAPEREDCAPSULE_OBJ_HEIGHT,
		PA_TAPEREDCAPSULE_OBJ_VERSION_INTERNAL,
	};
	taperParameters->GetValue(PA_TAPEREDCAPSULE_OBJ_RADIUS, 0, radius, FOREVER);
	

	int shapeType;
	if (IParamBlock2* shapeParameters = shapeMod->GetParamBlockByID(PB_SHAPE_MOD_PBLOCK)) {
		shapeParameters->GetValue(PA_SHAPE_MOD_SHAPE_TYPE,0,shapeType,FOREVER);
	}

	//Havok Shape
	bhkShapeRef shape;

	if (shapeType == 2) {

		// Capsule
		bhkCapsuleShapeRef capsule = new bhkCapsuleShape();
		capsule->SetRadius(radius/scale);
		capsule->SetRadius1(radius/scale);
		capsule->SetRadius2(radius/scale);
		float length; 
		taperParameters->GetValue(PA_TAPEREDCAPSULE_OBJ_HEIGHT, 0, length, FOREVER);
		//get the normal
		Matrix3 axis(true);
		ragdollParent->GetObjOffsetRot().MakeMatrix(axis);
		Point3 normalAx = axis.GetRow(2);
		//capsule center
		Point3 center = ragdollParent->GetObjOffsetPos();
		//min and max points
		Point3 pt1 = center - normalAx*(length/2);
		Point3 pt2 = center + normalAx*(length/2);

		capsule->SetFirstPoint(TOVECTOR3(pt1)/scale);
		capsule->SetSecondPoint(TOVECTOR3(pt2)/scale);
		capsule->SetMaterial(HAV_MAT_SKIN);

		shape = StaticCast<bhkShape>(capsule);
		
	}
	else {
		// Sphere
		//CalcBoundingSphere(node, tm.GetTrans(), radius, 0);

		bhkSphereShapeRef sphere = new bhkSphereShape();
		sphere->SetRadius(radius/scale);
		sphere->SetMaterial(HAV_MAT_SKIN);
		shape = StaticCast<bhkShape>(sphere);
	}

	bhkRigidBodyRef body;

	if (shape)
	{
		bhkBlendCollisionObjectRef blendObj = new bhkBlendCollisionObject();
		body = new bhkRigidBody();

		Matrix3 tm = ragdollParent->GetObjTMAfterWSM(0);
		
		//Calculate Object Offset Matrix
		Matrix3 otm(1);
		Point3 pos = ragdollParent->GetObjOffsetPos();
		otm.PreTranslate(pos);
		Quat quat = ragdollParent->GetObjOffsetRot();
		PreRotateMatrix(otm, quat);
		Matrix3 otmInvert = otm;
		otmInvert.Invert();

		//correct object tm
		Matrix3 tmbhk = otmInvert * tm;

		//set geometric parameters
		body->SetRotation(TOQUATXYZW(Quat(tmbhk).Invert()));
		body->SetTranslation(TOVECTOR4(tmbhk.GetTrans() / scale));
		body->SetCenter(TOVECTOR4(ragdollParent->GetObjOffsetPos())/scale);

		//set physics
		if (IParamBlock2* rbParameters = rbMod->GetParamBlockByID(PB_RB_MOD_PBLOCK)) {
			//These are fundamental parameters

			int lyr = NP_DEFAULT_HVK_LAYER;
			int mtl = NP_DEFAULT_HVK_MATERIAL;
			int msys = NP_DEFAULT_HVK_MOTION_SYSTEM;
			int qtype = NP_DEFAULT_HVK_QUALITY_TYPE;
			float mass = NP_DEFAULT_HVK_MASS;
			float lindamp = NP_DEFAULT_HVK_LINEAR_DAMPING;
			float angdamp = NP_DEFAULT_HVK_ANGULAR_DAMPING;
			float frict = NP_DEFAULT_HVK_FRICTION;
			float maxlinvel = NP_DEFAULT_HVK_MAX_LINEAR_VELOCITY;
			float maxangvel = NP_DEFAULT_HVK_MAX_ANGULAR_VELOCITY;
			float resti = NP_DEFAULT_HVK_RESTITUTION;
			float pendepth = NP_DEFAULT_HVK_PENETRATION_DEPTH;
			Point3 InertiaTensor;


			rbParameters->GetValue(PA_RB_MOD_MASS, 0, mass, FOREVER);
			rbParameters->GetValue(PA_RB_MOD_RESTITUTION, 0, resti, FOREVER);
			rbParameters->GetValue(PA_RB_MOD_FRICTION, 0, frict, FOREVER);
			rbParameters->GetValue(PA_RB_MOD_INERTIA_TENSOR, 0, InertiaTensor, FOREVER);


			rbParameters->GetValue(PA_RB_MOD_LINEAR_DAMPING, 0, lindamp, FOREVER);
			rbParameters->GetValue(PA_RB_MOD_CHANGE_ANGULAR_DAMPING, 0, angdamp, FOREVER);

			rbParameters->GetValue(PA_RB_MOD_MAX_LINEAR_VELOCITY, 0, maxlinvel, FOREVER);
			rbParameters->GetValue(PA_RB_MOD_MAX_ANGULAR_VELOCITY, 0, maxangvel, FOREVER);

			rbParameters->GetValue(PA_RB_MOD_ALLOWED_PENETRATION_DEPTH, 0, pendepth, FOREVER);
			rbParameters->GetValue(PA_RB_MOD_QUALITY_TYPE, 0, qtype, FOREVER);

			body->SetMass(mass);
			body->SetRestitution(resti);
			body->SetFriction(frict);
			body->SetLinearDamping(lindamp);
			body->SetMaxLinearVelocity(maxlinvel);
			body->SetMaxAngularVelocity(maxangvel);
			body->SetPenetrationDepth(pendepth);
			InertiaMatrix im;
			im[0][0] = InertiaTensor[0];
			im[1][1] = InertiaTensor[1];
			im[2][2] = InertiaTensor[2];

			body->SetInertia(im);

			/*switch (qtype) {
			case QT_FIXED:
				body->SetQualityType(MO_QUAL_FIXED);
				break;
			case QT_KEYFRAMED:
				body->SetQualityType(MO_QUAL_KEYFRAMED);
				break;
			case QT_DEBRIS:
				body->SetQualityType(MO_QUAL_DEBRIS);
				break;
			case QT_MOVING:
				body->SetQualityType(MO_QUAL_MOVING);
				break;
			case QT_CRITICAL:
				body->SetQualityType(MO_QUAL_CRITICAL);
				break;
			case QT_BULLET:
				body->SetQualityType(MO_QUAL_BULLET);
				break;
			case QT_KEYFRAMED_REPORTING:
				body->SetQualityType(MO_QUAL_KEYFRAMED_REPORT);
				break;
			}*/

			body->SetSkyrimLayer(SkyrimLayer::SKYL_BIPED);
			body->SetSkyrimLayerCopy(SkyrimLayer::SKYL_BIPED);

			body->SetMotionSystem(MotionSystem::MO_SYS_BOX);
			body->SetDeactivatorType(DeactivatorType::DEACTIVATOR_NEVER);
			body->SetSolverDeactivation(SolverDeactivation::SOLVER_DEACTIVATION_LOW);
			body->SetQualityType(MO_QUAL_FIXED);

		}
		
		if (constraintMod && ragdollParent->GetParentNode() && parent->GetParent()) {
			if (constraintMod->ClassID() == HK_CONSTRAINT_RAGDOLL_CLASS_ID) {
				bhkRagdollConstraintRef ragdollConstraint = new bhkRagdollConstraint();
				
				//entities
				ragdollConstraint->AddEntity(body);
				NiNodeRef parentRef = parent->GetParent();
				bhkRigidBodyRef nifParentRigidBody;
				while (parentRef) {
					if (parentRef->GetCollisionObject()) {
						nifParentRigidBody = StaticCast<bhkRigidBody>(StaticCast<bhkBlendCollisionObject>(parentRef->GetCollisionObject())->GetBody());
						break;
					}
					parentRef = parentRef->GetParent();
				}
				if (!nifParentRigidBody)
					throw exception(FormatText("Unable to find NIF constraint parent for ragdoll node %s", ragdollParent->GetName()));
				ragdollConstraint->AddEntity(nifParentRigidBody);

				RagdollDescriptor desc;
				//parameters
				if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) {
					Point3 pivotA;
					Matrix3 parentRotation;
					Point3 pivotB;
					Matrix3 childRotation;
					constraintParameters->GetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_TRANSLATION, 0, pivotB, FOREVER);
					constraintParameters->GetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, FOREVER);
					constraintParameters->GetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_TRANSLATION, 0, pivotA, FOREVER);
					constraintParameters->GetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, FOREVER);
					
					desc.pivotA = TOVECTOR4(pivotA);
					desc.pivotB = TOVECTOR4(pivotB);
					desc.planeA = TOVECTOR4(parentRotation.GetRow(0));
					desc.motorA = TOVECTOR4(parentRotation.GetRow(1));
					desc.twistA = TOVECTOR4(parentRotation.GetRow(2));
					desc.planeB = TOVECTOR4(childRotation.GetRow(0));
					desc.motorB = TOVECTOR4(childRotation.GetRow(1));
					desc.twistB = TOVECTOR4(childRotation.GetRow(2));
					
				}
				if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_RAGDOLL_MOD_PBLOCK)) {
					float coneMaxAngle;
					float planeMinAngle;
					float planeMaxAngle;
					float coneMinAngle;
					float twistMinAngle;
					float maxFriction;

					constraintParameters->GetValue(PA_RAGDOLL_MOD_CONE_ANGLE, 0, coneMaxAngle, FOREVER);
					constraintParameters->GetValue(PA_RAGDOLL_MOD_PLANE_MIN, 0, planeMinAngle, FOREVER);
					constraintParameters->GetValue(PA_RAGDOLL_MOD_PLANE_MAX, 0, planeMaxAngle, FOREVER);
					constraintParameters->GetValue(PA_RAGDOLL_MOD_TWIST_MIN, 0, coneMinAngle, FOREVER);
					constraintParameters->GetValue(PA_RAGDOLL_MOD_TWIST_MAX, 0, twistMinAngle, FOREVER);
					constraintParameters->GetValue(PA_RAGDOLL_MOD_MAX_FRICTION_TORQUE, 0, maxFriction, FOREVER);

					desc.coneMaxAngle = TORAD(coneMaxAngle);
					desc.planeMinAngle = TORAD(planeMinAngle);
					desc.planeMaxAngle = TORAD(planeMaxAngle);
					desc.coneMaxAngle = TORAD(coneMinAngle);
					desc.twistMinAngle = TORAD(twistMinAngle);
					desc.maxFriction = maxFriction;


				}
				ragdollConstraint->SetRagdoll(desc);
				body->AddConstraint(ragdollConstraint);
			}
			else if (constraintMod->ClassID() == HK_CONSTRAINT_HINGE_CLASS_ID) {
				bhkLimitedHingeConstraintRef limitedHingeConstraint = new bhkLimitedHingeConstraint();

				//entities
				limitedHingeConstraint->AddEntity(body);
				NiNodeRef parentRef = parent->GetParent();
				bhkRigidBodyRef nifParentRigidBody;
				while (parentRef) {
					if (parentRef->GetCollisionObject()) {
						nifParentRigidBody = StaticCast<bhkRigidBody>(StaticCast<bhkBlendCollisionObject>(parentRef->GetCollisionObject())->GetBody());
						break;
					}
					parentRef = parentRef->GetParent();
				}
				if (!nifParentRigidBody)
					throw exception(FormatText("Unable to find NIF constraint parent for limited hinge node %s", ragdollParent->GetName()));
				limitedHingeConstraint->AddEntity(nifParentRigidBody);

				LimitedHingeDescriptor lh;

				if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) {
					Matrix3 parentRotation;
					Matrix3 childRotation;
					constraintParameters->GetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, FOREVER);
					constraintParameters->GetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, FOREVER);

					lh.perp2AxleInA1 = TOVECTOR4(parentRotation.GetRow(0));
					lh.perp2AxleInA2 = TOVECTOR4(parentRotation.GetRow(1));
					lh.axleA = TOVECTOR4(parentRotation.GetRow(2));
					lh.perp2AxleInB1 = TOVECTOR4(childRotation.GetRow(0));
					lh.perp2AxleInB2 = TOVECTOR4(childRotation.GetRow(1));
					lh.axleB = TOVECTOR4(childRotation.GetRow(2));
					
				}
				if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_HINGE_MOD_PBLOCK)) {
					float minAngle;
					float maxAngle;
					float maxFriction;

					constraintParameters->GetValue(PA_HINGE_MOD_LIMIT_MIN, 0, minAngle, FOREVER);
					constraintParameters->GetValue(PA_HINGE_MOD_LIMIT_MAX, 0, maxAngle, FOREVER);
					constraintParameters->GetValue(PA_HINGE_MOD_MAX_FRICTION_TORQUE, 0, maxFriction, FOREVER);
					//	constraintParameters->SetValue(PA_HINGE_MOD_MOTOR_TYPE, 0, lh.motor., 0);

					lh.minAngle = TORAD(minAngle);
					lh.maxAngle = TORAD(maxAngle);
					lh.maxAngle = maxFriction;

				}
				limitedHingeConstraint->SetLimitedHinge(lh);
				body->AddConstraint(limitedHingeConstraint);
			}
		}


		//InitializeRigidBody(body, node);
		body->SetShape(shape);
		blendObj->SetBody(StaticCast<NiObject>(body));
		parent->SetCollisionObject(StaticCast<NiCollisionObject>(blendObj));
	}

	////rigid body parameters
	//	// get data from node
	//int lyr = NP_DEFAULT_HVK_LAYER;
	//int mtl = NP_DEFAULT_HVK_MATERIAL;
	//int msys = NP_DEFAULT_HVK_MOTION_SYSTEM;
	//int qtype = NP_DEFAULT_HVK_QUALITY_TYPE;
	//float mass = NP_DEFAULT_HVK_MASS;
	//float lindamp = NP_DEFAULT_HVK_LINEAR_DAMPING;
	//float angdamp = NP_DEFAULT_HVK_ANGULAR_DAMPING;
	//float frict = NP_DEFAULT_HVK_FRICTION;
	//float maxlinvel = NP_DEFAULT_HVK_MAX_LINEAR_VELOCITY;
	//float maxangvel = NP_DEFAULT_HVK_MAX_ANGULAR_VELOCITY;
	//float resti = NP_DEFAULT_HVK_RESTITUTION;
	//float pendepth = NP_DEFAULT_HVK_PENETRATION_DEPTH;
	//BOOL transenable = TRUE;

	//if (IParamBlock2* rbParameters = rbMod->GetParamBlockByID(PB_SHAPE_MOD_PBLOCK))
	//{
	//	//These are fundamental parameters
	//	rbParameters->GetValue(PA_RB_MOD_MASS, 0, mass, FOREVER);
	//	rbParameters->GetValue(PA_RB_MOD_RESTITUTION, 0, resti, FOREVER);
	//	rbParameters->GetValue(PA_RB_MOD_FRICTION, 0, frict, FOREVER);

	//	rbParameters->GetValue(PA_RB_MOD_LINEAR_DAMPING, 0, lindamp, FOREVER);
	//	rbParameters->GetValue(PA_RB_MOD_CHANGE_ANGULAR_DAMPING, 0, angdamp, FOREVER);

	//	rbParameters->GetValue(PA_RB_MOD_MAX_LINEAR_VELOCITY, 0, maxlinvel, FOREVER);
	//	rbParameters->GetValue(PA_RB_MOD_MAX_ANGULAR_VELOCITY, 0, maxangvel, FOREVER);

	//	rbParameters->GetValue(PA_RB_MOD_ALLOWED_PENETRATION_DEPTH, 0, pendepth, FOREVER);

	//	rbParameters->GetValue(PA_RB_MOD_QUALITY_TYPE, 0, qtype, FOREVER);


	//	switch (qtype) {
	//	case MO_QUAL_INVALID:
	//		break;
	//	case QT_FIXED:
	//		rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, MO_QUAL_FIXED, 0);
	//		break;
	//	case MO_QUAL_KEYFRAMED:
	//		rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_KEYFRAMED, 0);
	//		break;
	//	case MO_QUAL_DEBRIS:
	//		rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_DEBRIS, 0);
	//		break;
	//	case MO_QUAL_MOVING:
	//		rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_MOVING, 0);
	//		break;
	//	case MO_QUAL_CRITICAL:
	//		rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_CRITICAL, 0);
	//		break;
	//	case MO_QUAL_BULLET:
	//		rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_BULLET, 0);
	//		break;
	//	case MO_QUAL_USER:
	//		break;
	//	case MO_QUAL_CHARACTER:
	//		break;
	//	case MO_QUAL_KEYFRAMED_REPORT:
	//		rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_KEYFRAMED_REPORTING, 0);
	//		break;
	//	}

	//	// setup body
	//	bhkRigidBodyRef body = transenable ? new bhkRigidBodyT() : new bhkRigidBody();

	//	OblivionLayer obv_layer; SkyrimLayer sky_layer;
	//	GetHavokLayersFromIndex(lyr, (int*)&obv_layer, (int*)&sky_layer);
	//	body->SetLayer(obv_layer);
	//	body->SetLayerCopy(obv_layer);
	//	body->SetSkyrimLayer(sky_layer);

	//	body->SetMotionSystem(MotionSystem(msys));
	//	body->SetQualityType(MotionQuality(qtype));
	//	body->SetMass(mass);
	//	body->SetLinearDamping(lindamp);
	//	body->SetAngularDamping(angdamp);
	//	body->SetFriction(frict);
	//	body->SetRestitution(resti);
	//	body->SetMaxLinearVelocity(maxlinvel);
	//	body->SetMaxAngularVelocity(maxangvel);
	//	body->SetPenetrationDepth(pendepth);
	//	body->SetCenter(center);
	//	QuaternionXYZW q; q.x = q.y = q.z = 0; q.w = 1.0f;
	//	body->SetRotation(q);
	//}
}
Example #16
0
QwtText ScaleDraw::label(double value) const
{
    switch (d_type) {
    case Numeric:
    {
        QLocale locale = (static_cast<Graph *>(d_plot->parent()))->multiLayer()->locale();
        if (d_numeric_format == Superscripts) {
            QString txt = locale.toString(transformValue(value), 'e', d_prec);
            QStringList list = txt.split("e", QString::SkipEmptyParts);
            if (list[0].toDouble() == 0.0)
                return QString("0");

            QString s = list[1];
            int l = s.length();
            QChar sign = s[0];
            s.remove (sign);

            while (l>1 && s.startsWith ("0", false)) {
                s.remove ( 0, 1 );
                l = s.length();
            }

            if (sign == '-')
                s.prepend(sign);

            if (list[0] == "1")
                return QwtText("10<sup>" + s + "</sup>");
            else
                return QwtText(list[0] + "x10<sup>" + s + "</sup>");
        } else
            return QwtText(locale.toString(transformValue(value), d_fmt, d_prec));
        break;
    }

    case Day:
    {
        int val = static_cast<int>(transformValue(value))%7;
        if (val < 0)
            val = 7 - abs(val);
        else if (val == 0)
            val = 7;

        QString day;
        switch(d_name_format) {
        case  ShortName:
            day = QDate::shortDayName (val);
            break;
        case  LongName:
            day = QDate::longDayName (val);
            break;
        case  Initial:
            day = (QDate::shortDayName (val)).left(1);
            break;
        }
        return QwtText(day);
        break;
    }

    case Month:
    {
        int val = static_cast<int>(transformValue(value))%12;
        if (val < 0)
            val = 12 - abs(val);
        else if (val == 0)
            val = 12;

        QString day;
        switch(d_name_format) {
        case  ShortName:
            day = QDate::shortMonthName (val);
            break;
        case  LongName:
            day = QDate::longMonthName (val);
            break;
        case  Initial:
            day = (QDate::shortMonthName (val)).left(1);
            break;
        }
        return QwtText(day);
        break;
    }

    case Time:
        return QwtText(d_date_time_origin.time().addMSecs((int)value).toString(d_format_info));
        break;

    case Date:
        return QwtText(d_date_time_origin.addSecs((int)value).toString(d_format_info));
        break;

    case ColHeader:
    case Text:
    {
        const QwtScaleDiv scDiv = scaleDiv();
        if (!scDiv.contains (value))
            return QwtText();

        QwtValueList ticks = scDiv.ticks (QwtScaleDiv::MajorTick);

        double break_offset = 0;
        ScaleEngine *se = static_cast<ScaleEngine *>(d_plot->axisScaleEngine(axis()));
        /*QwtScaleEngine *qwtsc_engine=d_plot->axisScaleEngine(axis());
        ScaleEngine *se =dynamic_cast<ScaleEngine*>(qwtsc_engine);
        if(se!=NULL)
        {*/
        bool inverted = se->testAttribute(QwtScaleEngine::Inverted);
        if(se->hasBreak()) {
            double lb = se->axisBreakLeft();
            double rb = se->axisBreakRight();
            if(inverted) {
                if (value <= lb) {
                    int n_ticks = (int)ticks.count() - 1;
                    double val0 = ticks[0];
                    double val1 = ticks[n_ticks];
                    for (int i = 1; i < n_ticks; i++) {
                        double aux = ticks[i];
                        if(aux >= rb && val0 > aux) {
                            val0 = aux;
                            continue;
                        }
                        if(aux <= lb && val1 < aux)
                            val1 = aux;
                    }
                    break_offset = fabs(val1 - val0);
                }
            } else {
                if (value >= rb) {
                    double val0 = ticks[0];
                    for (int i = 1; i < (int)ticks.count(); i++) {
                        double val = ticks[i];
                        if(val0 <= lb && val >= rb) {
                            break_offset = fabs(val - val0);
                            break;
                        }
                        val0 = val;
                    }
                }
            }
        }

        double step = ticks[1] - ticks[0];
        int index = static_cast<int>(ticks[0] + step*ticks.indexOf(value) - 1);
        int offset = abs((int)floor(break_offset/step));
        if (offset)
            offset--;
        if (step > 0)
            index += offset;
        else
            index -= offset;
        if (index >= 0 && index < (int)d_text_labels.count())
            return QwtText(d_text_labels[index]);
        else
            return QwtText();
        //}
        break;
    }
    }
    return QwtText();
}
Example #17
0
File: Grid.cpp Project: PennTao/GPU
glm::mat4 update_rotation() {
	object_rotation += rotation_step;
	if (object_rotation >= 360.0f) object_rotation = 0.0f;
    glm::vec3 axis(0.0f,0.0f,1.0f);
	return glm::rotate(glm::mat4(), object_rotation, axis);
}
Example #18
0
//*************************************************************************************************************
void Render(float alpha, float elapsedtime)
{
	LPDIRECT3DSURFACE9 oldtarget = NULL;

	D3DXMATRIX		vp, inv, tmp1, tmp2;
	D3DXVECTOR3		axis(0, 1, 0);
	D3DXVECTOR3		eye(0, 0, -5);
	D3DXVECTOR3		look(0, 0, 0);
	D3DXVECTOR3		up(0, 1, 0);

	D3DXVECTOR2		cangle	= cameraangle.smooth(alpha);
	D3DXVECTOR2		oangle	= objectangle.smooth(alpha);
	float			expo	= exposure.smooth(alpha);

	D3DXMatrixRotationYawPitchRoll(&world, cangle.x, cangle.y, 0);
	D3DXVec3TransformCoord(&eye, &eye, &world);

	D3DXMatrixLookAtLH(&view, &eye, &look, &up);
	D3DXMatrixMultiply(&vp, &view, &proj);
	D3DXMatrixInverse(&inv, NULL, &view);

	memcpy(&eye, inv.m[3], 3 * sizeof(float));

	if( mesh == mesh1 )
	{
		// skullocc
		D3DXMatrixScaling(&world, 0.4f, 0.4f, 0.4f);
		world._42 = -1.5f;
	}
	else if( mesh == mesh2 )
	{
		// knot
		D3DXMatrixScaling(&world, 0.8f, 0.8f, 0.8f);
	}
	else
	{
		// teapot
		D3DXMatrixScaling(&world, 1.5f, 1.5f, 1.5f);
	}

	D3DXMatrixRotationYawPitchRoll(&tmp1, oangle.x, oangle.y, 0);
	D3DXMatrixMultiply(&world, &world, &tmp1);
	D3DXMatrixInverse(&inv, NULL, &world);

	fresnel->SetVector("eyePos", (D3DXVECTOR4*)&eye);
	fresnel->SetMatrix("matWorld", &world);
	fresnel->SetMatrix("matWorldInv", &inv);
	fresnel->SetMatrix("matViewProj", &vp);

	D3DXMatrixScaling(&world, 20, 20, 20);
	skyeffect->SetMatrix("matWorld", &world);

	D3DXMatrixIdentity(&world);
	skyeffect->SetMatrix("matWorldSky", &world);
	skyeffect->SetMatrix("matViewProj", &vp);

	memcpy(tmpvert, quadvertices, 36 * sizeof(float));

	if( SUCCEEDED(device->BeginScene()) )
	{
		device->SetRenderState(D3DRS_SRGBWRITEENABLE, false);
		device->SetSamplerState(0, D3DSAMP_ADDRESSU, D3DTADDRESS_WRAP);
		device->SetSamplerState(0, D3DSAMP_ADDRESSV, D3DTADDRESS_WRAP);

		// STEP 1: render sky
		device->GetRenderTarget(0, &oldtarget);

		if( firstframe )
		{
			device->SetRenderTarget(0, aftersurfaces[0]);
			device->Clear(0, NULL, D3DCLEAR_TARGET, 0, 1.0f, 0);

			device->SetRenderTarget(0, aftersurfaces[1]);
			device->Clear(0, NULL, D3DCLEAR_TARGET, 0, 1.0f, 0);

			device->SetRenderTarget(0, avglumsurfaces[4]);
			device->Clear(0, NULL, D3DCLEAR_TARGET, 0x11111111, 1.0f, 0);

			device->SetRenderTarget(0, avglumsurfaces[5]);
			device->Clear(0, NULL, D3DCLEAR_TARGET, 0x11111111, 1.0f, 0);

			firstframe = false;
		}

		device->SetRenderTarget(0, scenesurface);
		device->Clear(0, NULL, D3DCLEAR_TARGET|D3DCLEAR_ZBUFFER, 0xff6694ed, 1.0f, 0);
		device->SetRenderState(D3DRS_ZENABLE, FALSE);

		device->SetTexture(0, skytexture);

		skyeffect->Begin(NULL, 0);
		skyeffect->BeginPass(0);
		{
			skymesh->DrawSubset(0);
		}
		skyeffect->EndPass();
		skyeffect->End();
		
		device->SetRenderState(D3DRS_ZENABLE, TRUE);

		// STEP 2: render object
		device->SetTexture(0, texture);
		device->SetTexture(1, fresneltexture);
		device->SetTexture(2, skytexture);
		device->SetTexture(3, roughspecular);

		fresnel->Begin(NULL, 0);
		fresnel->BeginPass(0);
		{
			mesh->DrawSubset(0);
		}
		fresnel->EndPass();
		fresnel->End();

		device->SetVertexDeclaration(vertexdecl);

		// STEP 3: measure average luminance
		MeasureLuminance();

		// STEP 4: adapt luminance to eye
		AdaptLuminance(elapsedtime);

		// STEP 5: bright pass
		BrightPass();

		// STEP 6: downsample bright pass texture
		DownSample();

		// STEP 7: blur downsampled textures
		Blur();

		// STEP 8: ghost
		LensFlare();

		// STEP 9: star
		Star();

		// STEP 10: final combine
		hdreffect->SetTechnique("final");
		hdreffect->SetFloat("targetluminance", targetluminance);

		device->SetRenderTarget(0, oldtarget);
		device->SetTexture(0, scenetarget);			// scene
		device->SetTexture(1, blurtargets[0]);		// blur
		device->SetTexture(2, blurtargets[1]);		// star
		device->SetTexture(3, ghosttargets[0]);		// ghost
		device->SetTexture(4, afterimages[1 - afterimagetex]);

		device->SetSamplerState(0, D3DSAMP_ADDRESSU, D3DTADDRESS_WRAP);
		device->SetSamplerState(0, D3DSAMP_ADDRESSV, D3DTADDRESS_WRAP);
		device->SetRenderState(D3DRS_SRGBWRITEENABLE, true);

		oldtarget->Release();

		hdreffect->Begin(NULL, 0);
		hdreffect->BeginPass(0);
		{
			device->DrawPrimitiveUP(D3DPT_TRIANGLELIST, 2, quadvertices, sizeof(D3DXVECTOR4) + sizeof(D3DXVECTOR2));
		}
		hdreffect->EndPass();
		hdreffect->End();

		if( drawhelp )
		{
			// render text
			device->SetFVF(D3DFVF_XYZRHW|D3DFVF_TEX1);
			device->SetRenderState(D3DRS_ZENABLE, FALSE);
			device->SetRenderState(D3DRS_ALPHABLENDENABLE, TRUE);
			device->SetRenderState(D3DRS_SRCBLEND, D3DBLEND_SRCALPHA);
			device->SetRenderState(D3DRS_DESTBLEND, D3DBLEND_INVSRCALPHA);

			device->SetTexture(0, text);
			device->DrawPrimitiveUP(D3DPT_TRIANGLELIST, 2, textvertices, 6 * sizeof(float));

			device->SetRenderState(D3DRS_ALPHABLENDENABLE, FALSE);
			device->SetRenderState(D3DRS_ZENABLE, TRUE);
		}

		// clean up
		device->SetTexture(1, NULL);
		device->SetTexture(2, NULL);
		device->SetTexture(3, NULL);
		device->SetTexture(4, NULL);
		device->SetTexture(5, NULL);

		device->EndScene();
	}

	device->Present(NULL, NULL, NULL, NULL);
}
Example #19
0
SOrientedBoundingBox *
SOrientedBoundingBox::buildOBB(std::vector<SPoint3> &vertices)
{
#if defined(HAVE_MESH)

  int num_vertices = vertices.size();
  // First organize the data

  std::set<SPoint3> unique;
  unique.insert(vertices.begin(), vertices.end());

  num_vertices = unique.size();
  fullMatrix<double> data(3, num_vertices);

  fullVector<double> mean(3);
  fullVector<double> vmins(3);
  fullVector<double> vmaxs(3);

  mean.setAll(0);
  vmins.setAll(DBL_MAX);
  vmaxs.setAll(-DBL_MAX);

  size_t idx = 0;
  for(std::set<SPoint3>::iterator uIter = unique.begin(); uIter != unique.end();
      ++uIter) {
    const SPoint3 &pp = *uIter;
    for(int d = 0; d < 3; d++) {
      data(d, idx) = pp[d];
      vmins(d) = std::min(vmins(d), pp[d]);
      vmaxs(d) = std::max(vmaxs(d), pp[d]);
      mean(d) += pp[d];
    }
    idx++;
  }

  for(int i = 0; i < 3; i++) { mean(i) /= num_vertices; }

  // Get the deviation from the mean
  fullMatrix<double> B(3, num_vertices);
  for(int i = 0; i < 3; i++) {
    for(int j = 0; j < num_vertices; j++) { B(i, j) = data(i, j) - mean(i); }
  }

  // Compute the covariance matrix
  fullMatrix<double> covariance(3, 3);
  B.mult(B.transpose(), covariance);
  covariance.scale(1. / (num_vertices - 1));
  /*
  Msg::Debug("Covariance matrix");
  Msg::Debug("%f %f %f", covariance(0,0),covariance(0,1),covariance(0,2) );
  Msg::Debug("%f %f %f", covariance(1,0),covariance(1,1),covariance(1,2) );
  Msg::Debug("%f %f %f", covariance(2,0),covariance(2,1),covariance(2,2) );
  */
  for(int i = 0; i < 3; i++) {
    for(int j = 0; j < 3; j++) {
      if(std::abs(covariance(i, j)) < 10e-16) covariance(i, j) = 0;
    }
  }

  fullMatrix<double> left_eigv(3, 3);
  fullMatrix<double> right_eigv(3, 3);
  fullVector<double> real_eig(3);
  fullVector<double> img_eig(3);
  covariance.eig(real_eig, img_eig, left_eigv, right_eigv, true);

  // Now, project the data in the new basis.
  fullMatrix<double> projected(3, num_vertices);
  left_eigv.transpose().mult(data, projected);
  // Get the size of the box in the new direction
  fullVector<double> mins(3);
  fullVector<double> maxs(3);
  for(int i = 0; i < 3; i++) {
    mins(i) = DBL_MAX;
    maxs(i) = -DBL_MAX;
    for(int j = 0; j < num_vertices; j++) {
      maxs(i) = std::max(maxs(i), projected(i, j));
      mins(i) = std::min(mins(i), projected(i, j));
    }
  }

  // double means[3];
  double sizes[3];

  // Note:  the size is computed in the box's coordinates!
  for(int i = 0; i < 3; i++) {
    sizes[i] = maxs(i) - mins(i);
    // means[i] = (maxs(i) - mins(i)) / 2.;
  }

  if(sizes[0] == 0 && sizes[1] == 0) {
    // Entity is a straight line...
    SVector3 center;
    SVector3 Axis1;
    SVector3 Axis2;
    SVector3 Axis3;

    Axis1[0] = left_eigv(0, 0);
    Axis1[1] = left_eigv(1, 0);
    Axis1[2] = left_eigv(2, 0);
    Axis2[0] = left_eigv(0, 1);
    Axis2[1] = left_eigv(1, 1);
    Axis2[2] = left_eigv(2, 1);
    Axis3[0] = left_eigv(0, 2);
    Axis3[1] = left_eigv(1, 2);
    Axis3[2] = left_eigv(2, 2);

    center[0] = (vmaxs(0) + vmins(0)) / 2.0;
    center[1] = (vmaxs(1) + vmins(1)) / 2.0;
    center[2] = (vmaxs(2) + vmins(2)) / 2.0;

    return new SOrientedBoundingBox(center, sizes[0], sizes[1], sizes[2], Axis1,
                                    Axis2, Axis3);
  }

  // We take the smallest component, then project the data on the plane defined
  // by the other twos

  int smallest_comp = 0;
  if(sizes[0] <= sizes[1] && sizes[0] <= sizes[2])
    smallest_comp = 0;
  else if(sizes[1] <= sizes[0] && sizes[1] <= sizes[2])
    smallest_comp = 1;
  else if(sizes[2] <= sizes[0] && sizes[2] <= sizes[1])
    smallest_comp = 2;

  // The projection has been done circa line 161.
  // We just ignore the coordinate corresponding to smallest_comp.
  std::vector<SPoint2 *> points;
  for(int i = 0; i < num_vertices; i++) {
    SPoint2 *p = new SPoint2(projected(smallest_comp == 0 ? 1 : 0, i),
                             projected(smallest_comp == 2 ? 1 : 2, i));
    bool keep = true;
    for(std::vector<SPoint2 *>::iterator point = points.begin();
        point != points.end(); point++) {
      if(std::abs((*p)[0] - (**point)[0]) < 10e-10 &&
         std::abs((*p)[1] - (**point)[1]) < 10e-10) {
        keep = false;
        break;
      }
    }
    if(keep) { points.push_back(p); }
    else {
      delete p;
    }
  }

  // Find the convex hull from a delaunay triangulation of the points
  DocRecord record(points.size());
  record.numPoints = points.size();
  srand((unsigned)time(0));
  for(std::size_t i = 0; i < points.size(); i++) {
    record.points[i].where.h =
      points[i]->x() + (10e-6) * sizes[smallest_comp == 0 ? 1 : 0] *
                         (-0.5 + ((double)rand()) / RAND_MAX);
    record.points[i].where.v =
      points[i]->y() + (10e-6) * sizes[smallest_comp == 2 ? 1 : 0] *
                         (-0.5 + ((double)rand()) / RAND_MAX);
    record.points[i].adjacent = NULL;
  }

  try {
    record.MakeMeshWithPoints();
  } catch(const char *err) {
    Msg::Error("%s", err);
  }

  std::vector<Segment> convex_hull;
  for(int i = 0; i < record.numTriangles; i++) {
    Segment segs[3];
    segs[0].from = record.triangles[i].a;
    segs[0].to = record.triangles[i].b;
    segs[1].from = record.triangles[i].b;
    segs[1].to = record.triangles[i].c;
    segs[2].from = record.triangles[i].c;
    segs[2].to = record.triangles[i].a;

    for(int j = 0; j < 3; j++) {
      bool okay = true;
      for(std::vector<Segment>::iterator seg = convex_hull.begin();
          seg != convex_hull.end(); seg++) {
        if(((*seg).from == segs[j].from && (*seg).from == segs[j].to)
           // FIXME:
           // || ((*seg).from == segs[j].to && (*seg).from == segs[j].from)
        ) {
          convex_hull.erase(seg);
          okay = false;
          break;
        }
      }
      if(okay) { convex_hull.push_back(segs[j]); }
    }
  }

  // Now, examinate all the directions given by the edges of the convex hull
  // to find the one that lets us build the least-area bounding rectangle for
  // then points.
  fullVector<double> axis(2);
  axis(0) = 1;
  axis(1) = 0;
  fullVector<double> axis2(2);
  axis2(0) = 0;
  axis2(1) = 1;
  SOrientedBoundingRectangle least_rectangle;
  least_rectangle.center[0] = 0.0;
  least_rectangle.center[1] = 0.0;
  least_rectangle.size[0] = -1.0;
  least_rectangle.size[1] = 1.0;

  fullVector<double> segment(2);
  fullMatrix<double> rotation(2, 2);

  for(std::vector<Segment>::iterator seg = convex_hull.begin();
      seg != convex_hull.end(); seg++) {
    // segment(0) = record.points[(*seg).from].where.h -
    // record.points[(*seg).to].where.h;  segment(1) =
    // record.points[(*seg).from].where.v - record.points[(*seg).to].where.v;
    segment(0) = points[(*seg).from]->x() - points[(*seg).to]->x();
    segment(1) = points[(*seg).from]->y() - points[(*seg).to]->y();
    segment.scale(1.0 / segment.norm());

    double cosine = axis(0) * segment(0) + segment(1) * axis(1);
    double sine = axis(1) * segment(0) - segment(1) * axis(0);
    // double sine = axis(0)*segment(1) - segment(0)*axis(1);

    rotation(0, 0) = cosine;
    rotation(0, 1) = sine;
    rotation(1, 0) = -sine;
    rotation(1, 1) = cosine;

    // TODO C++11 std::numeric_limits<double>
    double max_x = -DBL_MAX;
    double min_x = DBL_MAX;
    double max_y = -DBL_MAX;
    double min_y = DBL_MAX;

    for(int i = 0; i < record.numPoints; i++) {
      fullVector<double> pnt(2);
      // pnt(0) = record.points[i].where.h;
      // pnt(1) = record.points[i].where.v;
      pnt(0) = points[i]->x();
      pnt(1) = points[i]->y();

      fullVector<double> rot_pnt(2);
      rotation.mult(pnt, rot_pnt);

      if(rot_pnt(0) < min_x) min_x = rot_pnt(0);
      if(rot_pnt(0) > max_x) max_x = rot_pnt(0);
      if(rot_pnt(1) < min_y) min_y = rot_pnt(1);
      if(rot_pnt(1) > max_y) max_y = rot_pnt(1);
    }

    /**/
    fullVector<double> center_rot(2);
    fullVector<double> center_before_rot(2);
    center_before_rot(0) = (max_x + min_x) / 2.0;
    center_before_rot(1) = (max_y + min_y) / 2.0;
    fullMatrix<double> rotation_inv(2, 2);

    rotation_inv(0, 0) = cosine;
    rotation_inv(0, 1) = -sine;
    rotation_inv(1, 0) = sine;
    rotation_inv(1, 1) = cosine;

    rotation_inv.mult(center_before_rot, center_rot);

    fullVector<double> axis_rot1(2);
    fullVector<double> axis_rot2(2);

    rotation_inv.mult(axis, axis_rot1);
    rotation_inv.mult(axis2, axis_rot2);

    if((least_rectangle.area() == -1) ||
       (max_x - min_x) * (max_y - min_y) < least_rectangle.area()) {
      least_rectangle.size[0] = max_x - min_x;
      least_rectangle.size[1] = max_y - min_y;
      least_rectangle.center[0] = (max_x + min_x) / 2.0;
      least_rectangle.center[1] = (max_y + min_y) / 2.0;
      least_rectangle.center[0] = center_rot(0);
      least_rectangle.center[1] = center_rot(1);
      least_rectangle.axisX[0] = axis_rot1(0);
      least_rectangle.axisX[1] = axis_rot1(1);
      //      least_rectangle.axisX[0] = segment(0);
      //      least_rectangle.axisX[1] = segment(1);
      least_rectangle.axisY[0] = axis_rot2(0);
      least_rectangle.axisY[1] = axis_rot2(1);
    }
  }
  // TODO C++11 std::numeric_limits<double>::min() / max()
  double min_pca = DBL_MAX;
  double max_pca = -DBL_MAX;
  for(int i = 0; i < num_vertices; i++) {
    min_pca = std::min(min_pca, projected(smallest_comp, i));
    max_pca = std::max(max_pca, projected(smallest_comp, i));
  }
  double center_pca = (max_pca + min_pca) / 2.0;
  double size_pca = (max_pca - min_pca);

  double raw_data[3][5];
  raw_data[0][0] = size_pca;
  raw_data[1][0] = least_rectangle.size[0];
  raw_data[2][0] = least_rectangle.size[1];

  raw_data[0][1] = center_pca;
  raw_data[1][1] = least_rectangle.center[0];
  raw_data[2][1] = least_rectangle.center[1];

  for(int i = 0; i < 3; i++) {
    raw_data[0][2 + i] = left_eigv(i, smallest_comp);
    raw_data[1][2 + i] =
      least_rectangle.axisX[0] * left_eigv(i, smallest_comp == 0 ? 1 : 0) +
      least_rectangle.axisX[1] * left_eigv(i, smallest_comp == 2 ? 1 : 2);
    raw_data[2][2 + i] =
      least_rectangle.axisY[0] * left_eigv(i, smallest_comp == 0 ? 1 : 0) +
      least_rectangle.axisY[1] * left_eigv(i, smallest_comp == 2 ? 1 : 2);
  }
  // Msg::Info("Test 1 : %f
  // %f",least_rectangle.center[0],least_rectangle.center[1]);
  // Msg::Info("Test 2 : %f
  // %f",least_rectangle.axisY[0],least_rectangle.axisY[1]);

  int tri[3];

  if(size_pca > least_rectangle.size[0]) {
    // P > R0
    if(size_pca > least_rectangle.size[1]) {
      // P > R1
      tri[0] = 0;
      if(least_rectangle.size[0] > least_rectangle.size[1]) {
        // R0 > R1
        tri[1] = 1;
        tri[2] = 2;
      }
      else {
        // R1 > R0
        tri[1] = 2;
        tri[2] = 1;
      }
    }
    else {
      // P < R1
      tri[0] = 2;
      tri[1] = 0;
      tri[2] = 1;
    }
  }
  else { // P < R0
    if(size_pca < least_rectangle.size[1]) {
      // P < R1
      tri[2] = 0;
      if(least_rectangle.size[0] > least_rectangle.size[1]) {
        tri[0] = 1;
        tri[1] = 2;
      }
      else {
        tri[0] = 2;
        tri[1] = 1;
      }
    }
    else {
      tri[0] = 1;
      tri[1] = 0;
      tri[2] = 2;
    }
  }

  SVector3 size;
  SVector3 center;
  SVector3 Axis1;
  SVector3 Axis2;
  SVector3 Axis3;

  for(int i = 0; i < 3; i++) {
    size[i] = raw_data[tri[i]][0];
    center[i] = raw_data[tri[i]][1];
    Axis1[i] = raw_data[tri[0]][2 + i];
    Axis2[i] = raw_data[tri[1]][2 + i];
    Axis3[i] = raw_data[tri[2]][2 + i];
  }

  SVector3 aux1;
  SVector3 aux2;
  SVector3 aux3;
  for(int i = 0; i < 3; i++) {
    aux1(i) = left_eigv(i, smallest_comp);
    aux2(i) = left_eigv(i, smallest_comp == 0 ? 1 : 0);
    aux3(i) = left_eigv(i, smallest_comp == 2 ? 1 : 2);
  }
  center = aux1 * center_pca + aux2 * least_rectangle.center[0] +
           aux3 * least_rectangle.center[1];
  // center[1] = -center[1];

  /*
  Msg::Info("Box center : %f %f %f",center[0],center[1],center[2]);
  Msg::Info("Box size : %f %f %f",size[0],size[1],size[2]);
  Msg::Info("Box axis 1 : %f %f %f",Axis1[0],Axis1[1],Axis1[2]);
  Msg::Info("Box axis 2 : %f %f %f",Axis2[0],Axis2[1],Axis2[2]);
  Msg::Info("Box axis 3 : %f %f %f",Axis3[0],Axis3[1],Axis3[2]);

  Msg::Info("Volume : %f", size[0]*size[1]*size[2]);
  */

  return new SOrientedBoundingBox(center, size[0], size[1], size[2], Axis1,
                                  Axis2, Axis3);
#else
  Msg::Error("SOrientedBoundingBox requires mesh module");
  return 0;
#endif
}
Example #20
0
// Read Symmetry file ======================================================
// crystal symmetry matices from http://cci.lbl.gov/asu_gallery/
int SymList::read_sym_file(FileName fn_sym)
{
    int i, j;
    FILE *fpoii;
    char line[80];
    char *auxstr;
    DOUBLE ang_incr, rot_ang;
    int  fold;
    Matrix2D<DOUBLE> L(4, 4), R(4, 4);
    Matrix1D<DOUBLE> axis(3);
    int pgGroup = 0, pgOrder = 0;
    std::vector<std::string> fileContent;

    //check if reserved word

    // Open file ---------------------------------------------------------
    if ((fpoii = fopen(fn_sym.c_str(), "r")) == NULL)
    {
        //check if reserved word and return group and order
        if (isSymmetryGroup(fn_sym, pgGroup, pgOrder))
        {
        	fill_symmetry_class(fn_sym, pgGroup, pgOrder, fileContent);
        }
        else
            REPORT_ERROR((std::string)"SymList::read_sym_file:Can't open file: "
                     + " or do not recognize symmetry group" + fn_sym);
    }
    else
    {
        while (fgets(line, 79, fpoii) != NULL)
        {
            if (line[0] == ';' || line[0] == '#' || line[0] == '\0')
            	continue;
			fileContent.push_back(line);
        }
        fclose(fpoii);
    }

    // Count the number of symmetries ------------------------------------
    true_symNo = 0;
    // count number of axis and mirror planes. It will help to identify
    // the crystallographic symmetry

    int no_axis, no_mirror_planes, no_inversion_points;
    no_axis = no_mirror_planes = no_inversion_points = 0;

    for (int n=0; n<fileContent.size(); n++)
    {
    	strcpy(line,fileContent[n].c_str());
        auxstr = firstToken(line);
        if (auxstr == NULL)
        {
            std::cout << line;
            std::cout << "Wrong line in symmetry file, the line is skipped\n";
            continue;
        }
        if (strcmp(auxstr, "rot_axis") == 0)
        {
            auxstr = nextToken();
            fold = textToInteger(auxstr);
            true_symNo += (fold - 1);
            no_axis++;
        }
        else if (strcmp(auxstr, "mirror_plane") == 0)
        {
            true_symNo++;
            no_mirror_planes++;
        }
        else if (strcmp(auxstr, "inversion") == 0)
        {
            true_symNo += 1;
            no_inversion_points = 1;
        }
    }
    // Ask for memory
    __L.resize(4*true_symNo, 4);
    __R.resize(4*true_symNo, 4);
    __chain_length.resize(true_symNo);
    __chain_length.initConstant(1);

    // Read symmetry parameters
    i = 0;
    for (int n=0; n<fileContent.size(); n++)
    {
        strcpy(line,fileContent[n].c_str());
        auxstr = firstToken(line);
        // Rotational axis ---------------------------------------------------
        if (strcmp(auxstr, "rot_axis") == 0)
        {
            auxstr = nextToken();
            fold = textToInteger(auxstr);
            auxstr = nextToken();
            XX(axis) = textToDOUBLE(auxstr);
            auxstr = nextToken();
            YY(axis) = textToDOUBLE(auxstr);
            auxstr = nextToken();
            ZZ(axis) = textToDOUBLE(auxstr);
            ang_incr = 360. / fold;
            L.initIdentity();
            for (j = 1, rot_ang = ang_incr; j < fold; j++, rot_ang += ang_incr)
            {
                rotation3DMatrix(rot_ang, axis, R);
                R.setSmallValuesToZero();
                set_matrices(i++, L, R.transpose());
            }
            __sym_elements++;
            // inversion ------------------------------------------------------
        }
        else if (strcmp(auxstr, "inversion") == 0)
        {
            L.initIdentity();
            L(2, 2) = -1;
            R.initIdentity();
            R(0, 0) = -1.;
            R(1, 1) = -1.;
            R(2, 2) = -1.;
            set_matrices(i++, L, R);
            __sym_elements++;
            // mirror plane -------------------------------------------------------------
        }
        else if (strcmp(auxstr, "mirror_plane") == 0)
        {
            auxstr = nextToken();
            XX(axis) = textToFloat(auxstr);
            auxstr = nextToken();
            YY(axis) = textToFloat(auxstr);
            auxstr = nextToken();
            ZZ(axis) = textToFloat(auxstr);
            L.initIdentity();
            L(2, 2) = -1;
            Matrix2D<DOUBLE> A;
            alignWithZ(axis,A);
            A = A.transpose();
            R = A * L * A.inv();
            L.initIdentity();
            set_matrices(i++, L, R);
            __sym_elements++;
        }
    }

    compute_subgroup();

    return pgGroup;
}
Example #21
0
 bool isAxis(unsigned id) const { return id >= axis(0) && id <= axis(15); }
bool ColladaConverter::saveAs(const char* filename)
{
	(void) filename;
	if (m_collada)
		{
			for (int i=0;i<m_numObjects;i++)
			{
				btAssert(m_colladadomNodes[i]);
				if (!m_colladadomNodes[i]->getTranslate_array().getCount())
				{
					domTranslate* transl = (domTranslate*) m_colladadomNodes[i]->createAndPlace("translate");
					transl->getValue().append(0.);
					transl->getValue().append(0.);
					transl->getValue().append(0.);
				}

				while (m_colladadomNodes[i]->getTranslate_array().getCount() > 1)
				{
					m_colladadomNodes[i]->removeFromParent(m_colladadomNodes[i]->getTranslate_array().get(1));
					//m_colladadomNodes[i]->getTranslate_array().removeIndex(1);
				}

				{

					btVector3 np = m_rigidBodies[i]->getWorldTransform().getOrigin();
					domFloat3 newPos = m_colladadomNodes[i]->getTranslate_array().get(0)->getValue();
					newPos.set(0,np[0]);
					newPos.set(1,np[1]);
					newPos.set(2,np[2]);
					m_colladadomNodes[i]->getTranslate_array().get(0)->setValue(newPos);

				}


				if (!m_colladadomNodes[i]->getRotate_array().getCount())
				{
					domRotate* rot = (domRotate*)m_colladadomNodes[i]->createAndPlace("rotate");
					rot->getValue().append(1.0);
					rot->getValue().append(0.0);
					rot->getValue().append(0.0);
					rot->getValue().append(0.0);
				}

				while (m_colladadomNodes[i]->getRotate_array().getCount()>1)
				{
					m_colladadomNodes[i]->removeFromParent(m_colladadomNodes[i]->getRotate_array().get(1));
					//m_colladadomNodes[i]->getRotate_array().removeIndex(1);

				}

				{
					btQuaternion quat = m_rigidBodies[i]->getCenterOfMassTransform().getRotation();
					btVector3 axis(quat.getX(),quat.getY(),quat.getZ());
					axis[3] = 0.f;
					//check for axis length
					btScalar len = axis.length2();
					if (len < SIMD_EPSILON*SIMD_EPSILON)
						axis = btVector3(1.f,0.f,0.f);
					else
						axis /= btSqrt(len);
					m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(0,axis[0]);
					m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(1,axis[1]);
					m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(2,axis[2]);
					m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(3,quat.getAngle()*SIMD_DEGS_PER_RAD);
				}

				while (m_colladadomNodes[i]->getMatrix_array().getCount())
				{
					m_colladadomNodes[i]->removeFromParent(m_colladadomNodes[i]->getMatrix_array().get(0));
					//m_colladadomNodes[i]->getMatrix_array().removeIndex(0);
				}
			}
			char	saveName[550];
			static int saveCount=1;
			sprintf(saveName,"%s%i",getLastFileName(),saveCount++);
			char* name = &saveName[0];
			if (name[0] == '/')
			{
				name = &saveName[1];
			}

			if (m_dom->getAsset()->getContributor_array().getCount())
			{
				if (!m_dom->getAsset()->getContributor_array().get(0)->getAuthor())
				{
					m_dom->getAsset()->getContributor_array().get(0)->createAndPlace("author");
				}

				m_dom->getAsset()->getContributor_array().get(0)->getAuthor()->setValue
					("http://bullet.sf.net Erwin Coumans");

				if (!m_dom->getAsset()->getContributor_array().get(0)->getAuthoring_tool())
				{
					m_dom->getAsset()->getContributor_array().get(0)->createAndPlace("authoring_tool");
				}

				m_dom->getAsset()->getContributor_array().get(0)->getAuthoring_tool()->setValue
#ifdef WIN32
					("Bullet ColladaPhysicsViewer-Win32-0.8");
#else
#ifdef __APPLE__
					("Bullet ColladaPhysicsViewer-MacOSX-0.8");
#else
					("Bullet ColladaPhysicsViewer-UnknownPlatform-0.8");
#endif
#endif
				if (!m_dom->getAsset()->getContributor_array().get(0)->getComments())
				{
					m_dom->getAsset()->getContributor_array().get(0)->createAndPlace("comments");
				}
				 m_dom->getAsset()->getContributor_array().get(0)->getComments()->setValue
					 ("Comments to Physics Forum at http://www.continuousphysics.com/Bullet/phpBB2/index.php");
			}

			m_collada->saveAs(name);
			return true;

		}
		return false;
}
bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B,const AABB& p_concave_hint,Vector3 *r_sep_axis) {

	if (p_shape_A->is_concave())
		return false;

	if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE) {

		Vector3 a,b;
		bool col = solve_distance_plane(p_shape_B,p_transform_B,p_shape_A,p_transform_A,a,b);
		r_point_A=b;
		r_point_B=a;
		return !col;

	} else if (p_shape_B->is_concave()) {

		if (p_shape_A->is_concave())
			return false;


		const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B);

		_ConcaveCollisionInfo cinfo;
		cinfo.transform_A=&p_transform_A;
		cinfo.shape_A=p_shape_A;
		cinfo.transform_B=&p_transform_B;
		cinfo.result_callback=NULL;
		cinfo.userdata=NULL;
		cinfo.swap_result=false;
		cinfo.collided=false;
		cinfo.collisions=0;
		cinfo.aabb_tests=0;
		cinfo.tested=false;

		Transform rel_transform = p_transform_A;
		rel_transform.origin-=p_transform_B.origin;

		//quickly compute a local AABB

		bool use_cc_hint=p_concave_hint!=AABB();
		AABB cc_hint_aabb;
		if (use_cc_hint) {
			cc_hint_aabb=p_concave_hint;
			cc_hint_aabb.pos-=p_transform_B.origin;
		}

		AABB local_aabb;
		for(int i=0;i<3;i++) {

		     Vector3 axis( p_transform_B.basis.get_axis(i) );
		     float axis_scale = 1.0/axis.length();
		     axis*=axis_scale;

		     float smin,smax;

		     if (use_cc_hint) {
			     cc_hint_aabb.project_range_in_plane(Plane(axis,0),smin,smax);
		     } else {
			     p_shape_A->project_range(axis,rel_transform,smin,smax);
		      }

		     smin*=axis_scale;
		     smax*=axis_scale;

		     local_aabb.pos[i]=smin;
		     local_aabb.size[i]=smax-smin;
		}


		concave_B->cull(local_aabb,concave_distance_callback,&cinfo);
		if (!cinfo.collided) {
//			print_line(itos(cinfo.tested));
			r_point_A=cinfo.close_A;
			r_point_B=cinfo.close_B;

		}

		//print_line("DIST AABB TESTS: "+itos(cinfo.aabb_tests));

		return !cinfo.collided;
	} else {

		return gjk_epa_calculate_distance(p_shape_A,p_transform_A,p_shape_B,p_transform_B,r_point_A,r_point_B); //should pass sepaxis..
	}


	return false;
}
int main()
{

	cout << "\n\n////////////////////////////////////////////////////" << endl;
    cout << "////////////////////////////////////////////////////" << endl;
    cout << "mainTestCrankFreePropagation2D. Running example..." << endl;
    cout << "///////////////////////////////////////////////////" << endl;
	cout << "///////////////////////////////////////////////////" << endl;
	
	
	fstream axis("axis.txt",ios::out);
	fstream out0("out0.txt",ios::out);
    fstream out1("out1.txt",ios::out);
	
	
	//////////////////
    //  Parameters  //
    //////////////////
	
	int Nr=520;
	int Nz=680;
	
	double dz=0.3;
	double dr=0.3;
	complex dt=complex(0.01,0.);
	
	
	int Ntime=100;
	int snap=10;
	
	//Gaussian parameters
    
	double Rmax  = Nr*dr;
    double rho0  = 0.;//Rmax/2.;
	double rho00 = 0.;
	double z0    = 0.;		
	double v0r   = 0.;//5.;
	double v0z   = 0.0;	
	double sigma = 5.;
	
	// Print out the information on the screen
	
    cout << "\nNr: " << Nr << endl;
    cout << "Nz: " << Nz << endl;
    cout << "dz: " << dz << endl;
    cout << "dt: " << dt << endl;
    cout << "Ntime: " << Ntime << endl;
    cout << "Snapshots: " << snap << endl;
    cout << "Rmax: " << Rmax << endl;
    cout << "rho0: " << rho0 << endl;
    cout << "rho00: " << rho00 << endl;
    cout << "z0: " << z0 << endl;
    cout << "v0r: " << v0r << endl;
    cout << "v0z: " << v0z << endl;
    cout << "Sigma: " << sigma << endl;

	
	////////////////////////
    //  Declare objects  //
    ///////////////////////
	
	
	HankelMatrix HH(Nr,Rmax);
	
	waveUniform2D w;
	w.initialize(HH,Nz,dz);

	
	//Initial test function	
	
	for(int j=0;j<HH.Nr;j++)
		for(int i=0;i<Nz;i++)
		{
			w.phi[w.index(j,i)]= exp(  -(w.r[j] - rho0)*(w.r[j] - rho0)/sigma/sigma -(w.z[i] - z0)*(w.z[i] - z0)/sigma/sigma )*exp(I* (v0r*(w.r[j] - rho0) + v0z*(w.z[i] - z0)) );
		}
	
	
	// Check the norm
	
	cout << "\n\nOriginal norm: " << w.norm() << endl;
    w.normalize();
    cout << "Initial norm: " << w.norm() << endl << endl;
	
	
	//////////////////////////////////////////
    //  Save initial wavefunction and axis  //
    //////////////////////////////////////////
    
	for(int j=0;j<Nr;j++)
		axis << w.r[j] << endl;
	
	for(int j=0;j<Nz;j++)
		axis << w.z[j] << endl;
	
    for(int j=0;j<Nr;j++)
		for(int i=0;i<Nz;i++)
			out0 << abs(conj(w.phi[w.index(j,i)])*w.phi[w.index(j,i)])*w.r[j] << endl;
	
	
	////////////////////////////
    //  Prepare Crank Arrays  //
    ////////////////////////////

	
	w.PrepareCrankArrays(dt);
	
	
	////////////////////////////
    //  Start temporal loop   //
    ////////////////////////////
	
	// This variables is for measure the time elapsed in propagation
	
	time_t start, end;
	double diff;
	
	time(&start);
	
	for (int ktime=0; ktime<Ntime; ktime++)
	{
		
		cout << "Loop number: " << ktime << endl;
		
		
		///////////////
        //  Evolve   //
        ///////////////
		
		
		w.Zprop(dt/2.);
		w.Rprop(  dt );
		w.Zprop(dt/2.);
		
		
		//cout << "Error norm: " << 1.-w.norm() << endl << endl;
		
		
		if(ktime%(Ntime/(snap-1)) == 0)
			for(int j=0;j<Nr;j++)
				for(int i=0;i<Nz;i++)
					out1 << abs(conj(w.phi[w.index(j,i)])*w.phi[w.index(j,i)])*w.r[j] << endl;	
		
		
		/*
		 if((ktime%(Ntime/snap))==0)
		 {				
		 for(int j=0;j<HH.Nr;j++)
		 for(int i=0;i<Nz;i++)
		 fprintf(out2,"%e \n",abs(w.phi[w.index(j,i)])*abs(w.phi[w.index(j,i)])*w.r[j]);     //Save wave function multiply by rho axis
		 
		 
		 double rexp=0.;
		 for(int j=0;j<HH.Nr;j++)
		 for(int i=0;i<Nz;i++)
		 rexp+= abs(w.phi[w.index(j,i)])*abs(w.phi[w.index(j,i)])*w.r[j]*w.r[j]*w.dz*w.dr;
		 
		 
		 
		 double zexp=0.;
		 for(int j=0;j<HH.Nr;j++)
		 for(int i=0;i<Nz;i++)
		 zexp+= abs(w.phi[w.index(j,i)])*abs(w.phi[w.index(j,i)])*w.z[i]*w.r[j]*w.dz*w.dr;
		 
		 fprintf(out2_,"%e %e %e\n",1.-w.norm(),rexp,zexp);
		 printf("%d %e\n",ktime,1.-w.norm()); 
		 }
		 */
		
		
		/////////////////////////
        //  Apply the absorber //
        /////////////////////////
		
		
		//w.absorber(0.1,0.,0.1,0.,1./6.);
		
		
	}
	
	time(&end);
	
	diff = difftime(end,start);
	
	cout << "Time took by propagation: " << diff << endl;
	
	axis.close();
	out0.close();
	out1.close();
	
}
Example #25
0
void PowerPlotter::plotDevice(Device* pDev, uint32_t index)
{
  auto evs = pDev->getPowerUsageEvents();
  mStartTime = max(0, mStartTime);
  mEndTime = min(pDev->getEnvironment()->getTimestamp(), mEndTime);
  dvec power;
  dvec time;

  // use offset from last render as a basis
  double currVal = 0.0;
  if (mStartOffsets[index] >= evs->size())
  {
    mStartOffsets[index] = evs->size() - 1;
  }
  auto it = evs->begin() + mStartOffsets[index];
  while (mStartOffsets[index] > 0 && it->timestamp > mStartTime)
  {
    it = evs->begin() + (--mStartOffsets[index]);
    if (it == evs->end())
    {
      break;
    }
    currVal = it->power_mA;
  }

  // find last point before start
  while (it != evs->end() && it->timestamp <= mStartTime) 
  {
    currVal = it->power_mA;
    it++;
  }
  mStartOffsets[index] = it - evs->begin();
  power.push_back(currVal);
  time.push_back(mStartTime);


  for (; it != evs->end(); it++)
  {
    auto powEv = *it;
    if (powEv.timestamp > mEndTime)
    {
      break;
    }
    if (powEv.timestamp > mStartTime)
    {
      power.push_back(currVal);
      time.push_back(powEv.timestamp - 1);
      power.push_back(powEv.power_mA);
      time.push_back(powEv.timestamp);
    }
    currVal = powEv.power_mA;
  }
  power.push_back(currVal);
  time.push_back(min(pDev->getEnvironment()->getTimestamp(), mEndTime));

  grid(mGrid); //flakey
  axis(mStartTime, mEndTime, 0, 15);
  if (time.size() > 1)
    plot(time, power);

  dvec txPower, rxPower;
  dvec timeStartEnd;

  txPower.push_back(RADIO_POWER_TX);
  txPower.push_back(RADIO_POWER_TX);

  rxPower.push_back(RADIO_POWER_RX);
  rxPower.push_back(RADIO_POWER_RX);
  timeStartEnd.push_back(mStartTime);
  timeStartEnd.push_back(mEndTime);

  plot(timeStartEnd, txPower);
  set(":");
  set("r");
  plot(timeStartEnd, rxPower);
  set(":");
  set("g");
}
Example #26
0
// pos is the position of its base.
void PrevailingWindDemo::createWindmill ( hkpWorld* world, const hkpWind* wind, const hkVector4& pos )
{
	const hkReal heightOfPole = 6.0f;
	const hkReal lengthOfShaft = 1.5f;
	// Proportion of shaft, from the propeller end, that it attaches to the pole
	const hkReal shaftPivotProportion = 0.2f;
	const hkReal shaftMass = 1.0f;
	const hkReal widthOfTailFin = 1.0f;
	const int numberOfBlades = 5;
	const hkReal bladeLength = 2.2f;
	const hkReal bladeWidth = 0.5f;
	const hkReal bladeMass = 0.2f;

	// We make the shaft-pivot constraint happy by ensuring the forces due to gravity are balanced.
	hkReal finMass;
	{
		const hkReal bladeMoment = ( numberOfBlades * bladeMass ) * ( bladeWidth + lengthOfShaft * shaftPivotProportion );
		const hkReal shaftMoment = shaftMass * ( 0.5f - shaftPivotProportion );
		const hkReal tailDistance = widthOfTailFin * 0.5f + lengthOfShaft * ( 1.0f - shaftPivotProportion );
		finMass = ( bladeMoment - shaftMoment ) / tailDistance;
	}

	hkpRigidBody* verticalPole;
	{
		hkpShape* shape = new hkpCylinderShape( hkVector4( 0.0f, 0.0f, 0.0f ), hkVector4( 0.0f, heightOfPole, 0.0f ), 0.1f );
		hkpRigidBodyCinfo info;
		{
			info.m_shape = shape;
			info.m_motionType = hkpMotion::MOTION_FIXED;
			info.m_position = pos;
		}
		verticalPole = new hkpRigidBody( info );
		world->addEntity( verticalPole );
		shape->removeReference();
	}
	
	hkpRigidBody* head;
	{
		hkArray<hkpShape*> shapeArray;
		hkArray<hkpMassElement> massElements;

		// Add the shaft and calculate its mass properties.
		hkpShape* shaft;
		{
			hkVector4 halfExtents( 0.1f, 0.1f, lengthOfShaft / 2.0f );
			
			shaft = new hkpBoxShape( halfExtents, 0.0f );
			shapeArray.pushBack( shaft );
			
			hkpMassElement shaftMassElement;
			hkpInertiaTensorComputer::computeBoxVolumeMassProperties( halfExtents, shaftMass, shaftMassElement.m_properties );
			massElements.pushBack( shaftMassElement );			
		}

		hkpShape* tailFin;
		{
			hkVector4 halfExtents( 0.1f, widthOfTailFin, widthOfTailFin * 0.5f );
			hkVector4 translate( 0.0f, 0.0f, (widthOfTailFin + lengthOfShaft) * 0.5f );
			hkpBoxShape* finShape = new hkpBoxShape( halfExtents, 0.0f );
			tailFin = new hkpConvexTranslateShape( finShape, translate );
			finShape->removeReference();

			shapeArray.pushBack( tailFin );

			hkpMassElement finMassElement;

			hkpInertiaTensorComputer::computeBoxVolumeMassProperties( halfExtents, finMass, finMassElement.m_properties );
			finMassElement.m_transform.setIdentity();
			finMassElement.m_transform.setTranslation( translate );
			massElements.pushBack( finMassElement );
		}

		hkpShape* shape = new hkpListShape( shapeArray.begin(), shapeArray.getSize() );

		hkpRigidBodyCinfo info;
		{
			hkpMassProperties massProperties;
			{
				hkpInertiaTensorComputer::combineMassProperties(massElements, massProperties);
			}
			info.m_shape = shape;
			info.m_motionType = hkpMotion::MOTION_DYNAMIC;
			info.m_mass = massProperties.m_mass;
			info.m_inertiaTensor = massProperties.m_inertiaTensor;
			info.m_centerOfMass = massProperties.m_centerOfMass;
			info.m_position.setAdd4( pos, hkVector4( 0.0f, heightOfPole + 0.2f, ( 0.5f - shaftPivotProportion ) * lengthOfShaft ) );
			//info.m_angularDamping = 0.4f;
		}
		
		head = new hkpRigidBody( info );
		tailFin->removeReference();
		shaft->removeReference();

		world->addEntity( head );
		
		hkpWindAction* action = new hkpWindAction( head, wind, 0.1f );
		world->addAction(action);
		action->removeReference();

		shape->removeReference();
	}

	// Create the constraint which keeps the pole on top.
	{
		hkpHingeConstraintData* hc = new hkpHingeConstraintData();
		hkVector4 pivot;
		pivot.setAdd4( pos, hkVector4(0.0f, heightOfPole, 0.0f) );
		
		hkVector4 axis( 0.0f, 1.0f, 0.0f );

		hc->setInWorldSpace(verticalPole->getTransform(), head->getTransform(), pivot, axis);
		
		hkpConstraintInstance* constraint = new hkpConstraintInstance( verticalPole, head, hc );
		world->addConstraint( constraint ); 
		constraint->removeReference();
		hc->removeReference();  
	}
	
	hkpRigidBody* propeller;
	{
		hkArray<hkpShape*> shapeArray;
		hkArray<hkpMassElement> massElements;

		for (int i = 0; i < numberOfBlades; i++)
		{
			hkpShape* blade;
			hkVector4 halfExtents( bladeWidth / 2.0f, bladeLength / 2.0f, 0.1f );
			hkTransform transform;
			{
				hkTransform transform0;
				{
					hkVector4 translate( bladeWidth * 0.48f, bladeLength * 0.6f, 0.0f );
					hkQuaternion rotation;
					{
						hkVector4 axis( 0.0f, 1.0f, 0.0f );
						rotation.setAxisAngle( axis, HK_REAL_PI * 0.25 );
						rotation.normalize();
					}
					transform0.setTranslation( translate );
					transform0.setRotation( rotation );
				}
				hkTransform transform1;
				{
					transform1.setIdentity();
					hkQuaternion rotation;
					{
						hkVector4 axis( 0.0f, 0.0f, 1.0f );
						rotation.setAxisAngle( axis, (HK_REAL_PI * 2.0f * i) / (hkReal) numberOfBlades );
						rotation.normalize();
					}
					transform1.setRotation( rotation );
				}
				transform.setMul( transform1, transform0 );
			}
			hkpBoxShape* bladeShape = new hkpBoxShape( halfExtents, 0.0f );
			blade = new hkpConvexTransformShape( bladeShape, transform );
			bladeShape->removeReference();

			shapeArray.pushBack( blade );

			hkpMassElement bladeMassElement;
			hkpInertiaTensorComputer::computeBoxVolumeMassProperties( halfExtents, bladeMass, bladeMassElement.m_properties );
			bladeMassElement.m_transform = transform;
			massElements.pushBack( bladeMassElement );
		}

		hkpShape* shape = new hkpListShape( shapeArray.begin(), shapeArray.getSize() );

		hkpRigidBodyCinfo info;
		{
			hkpMassProperties massProperties;
			{
				hkpInertiaTensorComputer::combineMassProperties(massElements, massProperties);
			}
			info.m_shape = shape;
			info.m_motionType = hkpMotion::MOTION_DYNAMIC;
			info.m_mass = massProperties.m_mass;
			info.m_inertiaTensor = massProperties.m_inertiaTensor;
			info.m_centerOfMass = massProperties.m_centerOfMass;
			info.m_position.setAdd4( pos, hkVector4( 0.0f, heightOfPole + 0.2f, - shaftPivotProportion * lengthOfShaft - ( bladeWidth ) ) );
			//info.m_angularDamping = 0.3f;
		}
		
		propeller = new hkpRigidBody( info );
		
		for ( int i = 0; i < numberOfBlades; ++i )
		{
			shapeArray[i]->removeReference();
		}

		world->addEntity( propeller );
		
		hkpWindAction* action = new hkpWindAction( propeller, wind, 0.1f );
		world->addAction(action);
		action->removeReference();

		shape->removeReference();
	}

	// Create the constraint which keeps propeller attached to the head.
	{
		hkpHingeConstraintData* hc = new hkpHingeConstraintData();
		hkVector4 pivot;
		pivot.setAdd4( pos, hkVector4(0.0f, heightOfPole + 0.2f, - shaftPivotProportion * lengthOfShaft - ( bladeWidth ) ) );
		
		hkVector4 axis( 0.0f, 0.0f, 1.0f );

		hc->setInWorldSpace( head->getTransform(), propeller->getTransform(), pivot, axis );
		
		hkpConstraintInstance* constraint = new hkpConstraintInstance( head, propeller, hc );
		world->addConstraint( constraint ); 
		constraint->removeReference();
		hc->removeReference();  
	}
	
	propeller->removeReference();
	head->removeReference();
	verticalPole->removeReference();
}
Example #27
0
void ScaleDraw::drawBreak(QPainter *painter) const
{
    ScaleEngine *sc_engine = static_cast<ScaleEngine *>(d_plot->axisScaleEngine(axis()));
    /*const QwtScaleEngine * qwtsc_engine=d_plot->axisScaleEngine(axis());
    const ScaleEngine *sc_engine =dynamic_cast<const ScaleEngine*>(qwtsc_engine);
    if(sc_engine!=NULL)
    {*/
    if (!sc_engine->hasBreak() || !sc_engine->hasBreakDecoration())
        return;

    painter->save();
    painter->setRenderHint(QPainter::Antialiasing);

    int len = majTickLength();

    QwtScaleMap scaleMap = map();
    const QwtMetricsMap metricsMap = QwtPainter::metricsMap();
    QPoint pos = this->pos();

    if ( !metricsMap.isIdentity() ) {
        QwtPainter::resetMetricsMap();
        pos = metricsMap.layoutToDevice(pos);

        if ( orientation() == Qt::Vertical ) {
            scaleMap.setPaintInterval(
                metricsMap.layoutToDeviceY((int)scaleMap.p1()),
                metricsMap.layoutToDeviceY((int)scaleMap.p2()));
            len = metricsMap.layoutToDeviceX(len);
        } else {
            scaleMap.setPaintInterval(
                metricsMap.layoutToDeviceX((int)scaleMap.p1()),
                metricsMap.layoutToDeviceX((int)scaleMap.p2()));
            len = metricsMap.layoutToDeviceY(len);
        }
    }

    int lval = scaleMap.transform(sc_engine->axisBreakLeft());
    int rval = scaleMap.transform(sc_engine->axisBreakRight());
    switch(alignment()) {
    case LeftScale:
        QwtPainter::drawLine(painter, pos.x(), lval, pos.x() - len, lval + len);
        QwtPainter::drawLine(painter, pos.x(), rval, pos.x() - len, rval + len);
        break;
    case RightScale:
        QwtPainter::drawLine(painter, pos.x(), lval, pos.x() + len, lval - len);
        QwtPainter::drawLine(painter, pos.x(), rval, pos.x() + len, rval - len);
        break;
    case BottomScale:
        QwtPainter::drawLine(painter, lval, pos.y(), lval - len, pos.y() + len);
        QwtPainter::drawLine(painter, rval, pos.y(), rval - len, pos.y() + len);
        break;
    case TopScale:
        QwtPainter::drawLine(painter, lval, pos.y(), lval + len, pos.y() - len);
        QwtPainter::drawLine(painter, rval, pos.y(), rval + len, pos.y() - len);
        break;
    }

    QwtPainter::setMetricsMap(metricsMap); // restore metrics map
    painter->restore();
    //}
}
Example #28
0
//----------------------------------------------------------------------------
int main( void ) {

// uncomment to log dynamics
//  Moby::Log<Moby::OutputToFile>::reporting_level = 7;

  // create a simulator
  boost::shared_ptr<Moby::Simulator> sim( new Moby::Simulator() );

  // create a gravity vector
  boost::shared_ptr<Moby::GravityForce> g( new Moby::GravityForce() );
  g->gravity = Ravelin::Vector3d( 0, 0, -9.8 );

  // create an articulated body for a kinematic chain
  Moby::RCArticulatedBodyPtr pendulum( new Moby::RCArticulatedBody() );
  pendulum->id = "pendulum";
  pendulum->algorithm_type = Moby::RCArticulatedBody::eCRB;

  // vectors for references used in defining the body
  std::vector< Moby::RigidBodyPtr > links;
  std::vector< Moby::JointPtr > joints;

  // create the static base;
  Moby::RigidBodyPtr base( new Moby::RigidBody() );
  {
    // create a primitive box for inertia and rendering prototypes
    Moby::PrimitivePtr box( new Moby::BoxPrimitive(0.1,0.1,0.1) );
    box->set_mass( 1 );

    // assign the static base parameters
    base->id = "base";                        // identify the link
    base->set_visualization_data( box->create_visualization() );  // attach a visualization for osg
    base->set_inertia( box->get_inertia() );  // use the inertia of the box as a model
    base->set_enabled( false );               // disable physics for the base (this makes it static)
 
    // compute the pose of the base 
    Ravelin::Quatd rotation(Ravelin::Quatd::normalize(Ravelin::Quatd(0,0,0,1)));
    Ravelin::Origin3d position(0,0,0);
    Ravelin::Pose3d pose( rotation, position );
    base->set_pose( pose ); 

    // add the base to the set of links
    links.push_back( base );
  }

  // create the dynamic arm link
  Moby::RigidBodyPtr arm( new Moby::RigidBody() );
  {
    // create a primitive cylinder for inertia and rendering prototypes
    Moby::PrimitivePtr cylinder( new Moby::CylinderPrimitive(0.025,1) );
    cylinder->set_mass( 1 );

    // assign the arm parameters
    arm->id = "arm";                              // identify the link
    arm->set_visualization_data( cylinder->create_visualization() );  // attach a visualization for osg
    arm->set_inertia( cylinder->get_inertia() );  // use the inertia of the cylinder as a model
    arm->set_enabled( true );                     // enable physics for the arm
    arm->get_recurrent_forces().push_back( g );   // add the gravity force to the arm
 
    // compute the pose of the arm 
    Ravelin::Quatd rotation(Ravelin::Quatd::normalize(Ravelin::Quatd(0,0,0,1)));
    Ravelin::Origin3d position(0,-0.5,0);
    Ravelin::Pose3d pose( rotation, position );
    arm->set_pose( pose ); 

    // add the arm to the set of links
    links.push_back( arm ); 
  }

  // create the pivot joint
  boost::shared_ptr<Moby::RevoluteJoint> pivot( new Moby::RevoluteJoint() );
  {
    // compute the position of the joint... center of the base w.r.t global frame
    Ravelin::Pose3d pose = *base->get_pose();
    Ravelin::Vector3d position(pose.x.x(), pose.x.y(), pose.x.z(), Moby::GLOBAL);

    // compute the axis of rotation
    Ravelin::Vector3d axis(1,0,0,Moby::GLOBAL);   // actuates around global x-axis

    // assign the pivot parameters
    // Note: set_location(...) must precede set_axis(...)
    pivot->id = "pivot";
    pivot->set_location( position, base, arm );
    pivot->set_axis( axis ); 

    // add the pivot to the set of joints
    joints.push_back( pivot );
  }

  // construct the pendulum articulated body from the set of links and joints created above
  pendulum->set_links_and_joints( links, joints );
  // add gravity as force to affect the pendulum
  pendulum->get_recurrent_forces().push_back( g );
  // pendulum has a fixed base
  pendulum->set_floating_base(false);
 
  // add the pendulum to the simulaiton
  sim->add_dynamic_body( pendulum );

  // create a viewer to visualize the simulation (Must have built with OSG support to see)
  Viewer viewer( sim, Ravelin::Origin3d(-5,0,-1), Ravelin::Origin3d(0,0,-1), Ravelin::Origin3d(0,0,1) );

  // start the main simulation loop
  while(true) {
    // update the viewer, if the viewer fails to update then it has been closed so exit
    if( !viewer.update() ) { 
      break;
    }
    // step the simulation forward one millisecond
    sim->step( 0.001 );

    // get an updated arm pose w.r.t the global frame and print it to the console 
    Ravelin::Pose3d pose = *arm->get_pose();
    pose.update_relative_pose(Moby::GLOBAL);
    std::cout << "t: " << sim->current_time << ", pose: " << pose << std::endl;
  }

  return 0;
}
Example #29
0
//-- RotateAxis ---------------------------------------------------------------
//
//-----------------------------------------------------------------------------
void Renderer::RotateAxis( float angle, float x, float y, float z )
{
	D3DXVECTOR3 axis( x, y, z );
	g_matrixStack->RotateAxisLocal( &axis, D3DXToRadian( angle ) );

} // RotateAxis