void DG_Rotatef(float angle, float x, float y, float z)
{
D3DXVECTOR3 axis(x, y, z);
matStack[msIndex]->RotateAxisLocal(&axis, D3DXToRadian(angle));
UploadMatrix();
}
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);
}
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();
}
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();
}
}
}
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);
}
}
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);
}
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;
}
// 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;
}
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]);
}
}
}
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);
//}
}
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();
}
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);
}
//*************************************************************************************************************
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);
}
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
}
// 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;
}
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();
}
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");
}
// 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();
}
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();
//}
}
//----------------------------------------------------------------------------
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;
}
//-- RotateAxis ---------------------------------------------------------------
//
//-----------------------------------------------------------------------------
void Renderer::RotateAxis( float angle, float x, float y, float z )
{
D3DXVECTOR3 axis( x, y, z );
g_matrixStack->RotateAxisLocal( &axis, D3DXToRadian( angle ) );
} // RotateAxis
