glm::mat4 SimpleCam::updateDelta(const float *positionVec3, const float *rotationVec3)
{
// Rotation Axis
glm::vec3 xAxis(1.0f, 0.0f, 0.0f);
glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
glm::vec3 zAxis(0.0f, 0.0f, 1.0f); // TODO: should this be -1?
// Accumulate Rotations
m_rotation.x += rotationVec3[0]; // pitch
m_rotation.y += rotationVec3[1]; // yaw
m_rotation.z += rotationVec3[2]; // roll
// Calculate Rotation
glm::mat4 rotViewMat;
rotViewMat = glm::rotate(rotViewMat, m_rotation.x, xAxis);
rotViewMat = glm::rotate(rotViewMat, m_rotation.y, yAxis);
rotViewMat = glm::rotate(rotViewMat, m_rotation.z, zAxis);
// Updated direction vectors
m_forward = glm::vec3(rotViewMat[0][2], rotViewMat[1][2], rotViewMat[2][2]);
m_up = glm::vec3(rotViewMat[0][1], rotViewMat[1][1], rotViewMat[2][1]);
m_right = glm::vec3(rotViewMat[0][0], rotViewMat[1][0], rotViewMat[2][0]);
m_forward = glm::normalize(m_forward);
m_up = glm::normalize(m_up);
m_right = glm::normalize(m_right);
// Calculate Position
m_position += (m_forward * positionVec3[2]);
m_position += (m_up * positionVec3[1]);
m_position += (m_right * positionVec3[0]);
glm::mat4 translateViewMat;
translateViewMat = glm::translate(translateViewMat, m_position);
// Calculate view matrix.
m_viewMat = rotViewMat * translateViewMat;
// Return View Proj
return getViewProjMat();
}
/******************************************************************************
Draws the force vector.
******************************************************************************/
void drawForceVector(GLUquadricObj* pQuadObj,
const hduVector3Dd &position,
const hduVector3Dd &forceVector,
double arrowThickness)
{
glDisable(GL_LIGHTING);
glPushMatrix();
glTranslatef(position[0], position[1], position[2]);
// Change the force magnitude/direction by rotating the force vector.
// Calculate the rotation angle.
hduVector3Dd unitForceVectorAxis = normalize(forceVector);
hduVector3Dd zAxis( 0.0, 0.0, 1.0 );
hduVector3Dd toolRotAxis = zAxis.crossProduct(unitForceVectorAxis);
double toolRotAngle = acos(unitForceVectorAxis[2]);
hduMatrix rotMatrix = hduMatrix::createRotation(toolRotAxis,
toolRotAngle);
double rotVals[4][4];
rotMatrix.get(rotVals);
glMultMatrixd((double*) rotVals);
// The force arrow: composed of a cylinder and a cone.
glColor3f( 0.2, 0.7, 0.2 );
double strength = forceVector.magnitude();
// Draw arrow shaft.
gluCylinder(pQuadObj,arrowThickness, arrowThickness, strength, 16, 2);
glTranslatef(0, 0, strength);
glColor3f(0.2, 0.8, 0.3);
// Draw arrow head.
gluCylinder(pQuadObj, arrowThickness*2, 0.0, strength*.15, 16, 2);
glPopMatrix();
}
osg::Group* Cylinder::toGeometry(const osg::Vec4& color)
{
osg::Group* group = new osg::Group;
if (_vecPointIdx.size() < 2) {
return group;
}
Kernel::Line_3 axis(_point, _normal);
double min = std::numeric_limits<double>::max();
double max = std::numeric_limits<double>::min();
for (size_t i = 0, iEnd = _vecPointIdx.size(); i < iEnd; ++ i) {
Point projection = axis.projection(_vecPointSet[_vecPointIdx[i]]->point);
Vector vector(_point, projection);
double d = vector*_normal;
max = max<d ? d:max;
min = min>d ? d:min;
}
Point top = _point + max*_normal;
Point bottom = _point + min*_normal;
Point center = CGAL::midpoint(top, bottom);
osg::Vec3 offset = Vec3Caster<Vector>(top-bottom);
osg::Vec3 zAxis(0.0, 0.0, 1.0);
osg::Vec3 rotation = zAxis^offset;
float angle = acos((zAxis*offset)/offset.length());
osg::Cylinder* cylinder = new osg::Cylinder(Vec3Caster<Point>(center), _radius, offset.length());
cylinder->setRotation(osg::Quat(angle, rotation));
osg::Geode* geode = new osg::Geode;
osg::ShapeDrawable* drawable = new osg::ShapeDrawable(cylinder);
osg::Vec4 transparentColor = color;
transparentColor.a() = 0.6f;
drawable->setColor(transparentColor);
geode->addDrawable(drawable);
group->addChild(geode);
return group;
}
int Sphere::testRay(const sf::Vector3f& origin, const sf::Vector3f& direction, sf::Vector3f& hitPosition, sf::Vector2f& UVCoordinates)
{
float factor = -dot((origin-transform.position), direction);
sf::Vector3f x = origin + factor * direction;
float distance = sqrt((transform.scale.x/2 * transform.scale.x/2) - squaredLength(x-transform.position));
if(factor > distance)
{
hitPosition = origin + (factor - distance) * direction;
sf::Vector2f UVs(0.0f, 0.0f);
if(material.texture)
{
sf::Vector3f yAxis(0.0f, 1.0f, 0.0f);
sf::Vector3f zAxis(0.0f, 0.0f, -1.0f);
sf::Vector3f normal = getNormal(hitPosition);
float phi = acos( -dot( yAxis, normal ));
UVs.y = phi / 3.14f;
float theta = (acos( dot( normal, zAxis) / sin( phi ))) / ( 2 * 3.14f);
if ( dot(cross( yAxis, zAxis), normal ) > 0 )
{
UVs.x = theta;
}
else
{
UVs.x = 1 - theta;
}
}
UVCoordinates = UVs;
return 1;
}
else if(factor + distance > 0)
{
hitPosition = origin + (factor + distance) * direction;
return -1;
}
else
{
return 0;
}
}
bool IsOrthogonal(const Matrix4 &m)
{
// Axis components
Vector3 xAxis(m.xX, m.xY, m.xZ);
Vector3 yAxis(m.yX, m.yY, m.yZ);
Vector3 zAxis(m.zX, m.zY, m.zZ);
// Determine if all axis are perpendicular to each other
if (IsOrthonormal(m))
{
// Axis are perpendicular so now check to see if axis are unit vectors...
if (Equal(xAxis.Magnitude(), 1) == true
&& Equal(yAxis.Magnitude(), 1) == true
&& Equal(zAxis.Magnitude(), 1) == true)
{
// Whew! We have an Orthagonal Matrix4, too much effort if you ask me.
return true;
}
}
// Nwaaah! Its not orthogonal...
return false;
}
bool IsOrthonormal(const Matrix4 &m)
{
// Axis components
Vector3 xAxis(m.xX, m.xY, m.xZ);
Vector3 yAxis(m.yX, m.yY, m.yZ);
Vector3 zAxis(m.zX, m.zY, m.zZ);
// Angle relation ships between the axis
float xyTheta = FindAngle(xAxis, yAxis);
float yzTheta = FindAngle(yAxis, zAxis);
float xzTheta = FindAngle(xAxis, zAxis);
// Determine if all axis are perpendicular to each other
if (Equal(xyTheta, 90.0f) == true
&& Equal(yzTheta, 90.0f) == true
&& Equal(xzTheta, 90.0f) == true)
{
// Orthonormal Matrix4 as all axis are perpidicular.
return true;
}
// Fall through if axis are not perpindicular
return false;
}
void AccelSensor::setup() {
SimObject::setup();
if(mInitialized == false) {
mHostBody = Physics::getPhysicsManager()->getSimBody(mReferenceBodyName->get());
if(mHostBody == 0) {
Core::log("AccelSensor: Sensor has no valid reference body. Name was ["
+ mReferenceBodyName->get() + "]! Ignoring", true);
return;
}
createSensor();
mFirstSensorValue->set(0.0);
mSecondSensorValue->set(0.0);
mThirdSensorValue->set(0.0);
mTimeStepSize = dynamic_cast<DoubleValue*>(Core::getInstance()->
getValueManager()->getValue(SimulationConstants::VALUE_TIME_STEP_SIZE));
DoubleValue *mGravitationValue = dynamic_cast<DoubleValue*>(Core::getInstance()->
getValueManager()->getValue("/Simulation/Gravitation"));
if(mTimeStepSize == 0 || mGravitationValue == 0) {
Core::log("AccelSensor: Required Event or Value could not be found.");
return;
}
mGravitation = mGravitationValue->get();
mLastAxisOneMeasurement = 0.0;
mLastAxisTwoMeasurement = 0.0;
mLastAxisThreeMeasurement = 0.0;
mInitialized = true;
}
// rotate chosen axis about the localRotation of this sensor
Quaternion localOrientation = mLocalOrientation->get();
localOrientation.normalize();
mLocalOrientation->set(localOrientation);
Quaternion localOrientationInverse = localOrientation.getInverse();
localOrientationInverse.normalize();
Quaternion xAxis(0.0,
mSensorAxisOneValue->getX(),
mSensorAxisOneValue->getY(),
mSensorAxisOneValue->getZ());
Quaternion xAxisRotated = localOrientation * xAxis * localOrientationInverse;
mSensorAxisOne.set(xAxisRotated.getX(), xAxisRotated.getY(), xAxisRotated.getZ());
mRotatedSensorAxisOne.set(xAxisRotated.getX(),
xAxisRotated.getY(),
xAxisRotated.getZ());
Quaternion yAxis(0.0,
mSensorAxisTwoValue->getX(),
mSensorAxisTwoValue->getY(),
mSensorAxisTwoValue->getZ());
Quaternion yAxisRotated = localOrientation * yAxis * localOrientationInverse;
mSensorAxisTwo.set(yAxisRotated.getX(), yAxisRotated.getY(), yAxisRotated.getZ());
mRotatedSensorAxisTwo.set(yAxisRotated.getX(),
yAxisRotated.getY(),
yAxisRotated.getZ());
Quaternion zAxis(0.0,
mSensorAxisThreeValue->getX(),
mSensorAxisThreeValue->getY(),
mSensorAxisThreeValue->getZ());
Quaternion zAxisRotated = localOrientation * zAxis * localOrientationInverse;
mSensorAxisThree.set(zAxisRotated.getX(), zAxisRotated.getY(), zAxisRotated.getZ());
mRotatedSensorAxisThree.set(zAxisRotated.getX(),
zAxisRotated.getY(),
zAxisRotated.getZ());
if(mSensorBody == 0 || mHostBody == 0) {
Core::log("AccelSensor: The sensor has no valid host or sensor-body.");
return;
}
mSensorBody->setTextureType("None");
mSensorBody->setFaceTexture(5, "AccelBoard");
mSensorGeometry->setLocalOrientation(mLocalOrientation->get());
mSensorBody->getGeometry()->setLocalPosition(mLocalPosition->get());
Quaternion localPos(0,
mLocalPosition->getX(),
mLocalPosition->getY(),
mLocalPosition->getZ());
Quaternion bodyOrientationInverse = mHostBody->getQuaternionOrientationValue()->get().getInverse();
Quaternion rotatedLocalPosQuat = mHostBody->getQuaternionOrientationValue()->get()
* localPos * bodyOrientationInverse;
Vector3D rotatedLocalPos(rotatedLocalPosQuat.getX(),
rotatedLocalPosQuat.getY(),
rotatedLocalPosQuat.getZ());
mLastPosition = mHostBody->getPositionValue()->get() + rotatedLocalPos;
if(mLowPassFilterDelayValue != 0) {
mLowPassFilterDelay = mLowPassFilterDelayValue->get();
}
mValueOneHistory.clear();
mValueTwoHistory.clear();
mValueThreeHistory.clear();
mGlobalNoiseDeviation = mGlobalNoiseDeviationValue->get();
}
void Pulley::finishComponent() {
Vector3 wheel(getNode(1)->getTranslationWorld());
unsigned short i, j, v = 0;
std::vector<MyNode*> links(_numLinks);
std::vector<Vector3> joints(_numLinks+1);
//create the chain by duplicating a box a bunch of times
float x, y, z = wheel.z, angle;
Vector3 zAxis(0, 0, 1), joint, trans1, trans2;
joints[0].set(wheel);
joints[0].x -= _radius;
joints[0].y -= (_dropLinks + 0.5f) * _linkLength;
MyNode *link;
for(i = 0; i < _numLinks; i++) {
link = app->duplicateModelNode("box");
std::stringstream ss;
ss << _id << _typeCount << "_link" << (i+1);
const std::string nodeID = ss.str();
link->setId(nodeID.c_str());
if(i < _dropLinks) { //chain going up from left bucket
x = wheel.x - _radius;
y = wheel.y - (_dropLinks-i) * _linkLength;
angle = 0;
} else if(i < _dropLinks + _wheelLinks+1) { //chain going over wheel
angle = (i - _dropLinks) * (M_PI / _wheelLinks);
x = wheel.x -_radius * cos(angle);
y = wheel.y + _radius * sin(angle);
} else { //chain going down to right bucket
x = wheel.x + _radius;
y = wheel.y - (_dropLinks - (_numLinks-1 - i)) * _linkLength;
angle = M_PI;
}
link->setScale(_linkWidth/3, _linkLength/3, _linkWidth/3);
link->rotate(zAxis, -angle);
link->setTranslation(x, y, z);
link->addCollisionObject();
links[i] = link;
_rootNode->addChild(link);
//note the position of the joint between this link and the next
joint.set(0, (_linkLength/2) / link->getScaleY(), 0);
link->getWorldMatrix().transformPoint(&joint);
joints[i+1].set(joint);
}
//must enable all collision objects before adding constraints to prevent crashes
for(i = 0; i < _elements.size(); i++) {
getNode(i)->getCollisionObject()->setEnabled(true);
}
//connect each pair of adjacent links with a socket constraint
PhysicsSocketConstraint *constraint;
Quaternion rot1, rot2;
for(i = 0; i < _numLinks-1; i++) {
trans1.set(0, (_linkLength/2) / links[i]->getScaleY(), 0);
trans2.set(0, -(_linkLength/2) / links[i]->getScaleY(), 0);
constraint = (PhysicsSocketConstraint*) app->addConstraint(links[i], links[i+1], -1, "socket",
joints[i+1], Vector3::unitZ());
_constraints.push_back(constraint);
}
//connect each bucket to the adjacent chain link with a socket constraint
for(i = 0; i < 2; i++) {
constraint = (PhysicsSocketConstraint*) app->addConstraint(links[i * (_numLinks-1)], getNode(i+2), -1, "socket",
joints[i * _numLinks], Vector3::unitZ());
_constraints.push_back(constraint);
}
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("scene0003.pcd", *cloud) != 0)
{
return -1;
}
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ExtractIndices<pcl::PointXYZ> extract;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.filter(*plane);
if (inliers->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model for the given dataset.");
return (-1);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
std::cerr << "Plane cloud points: " << plane->size () << std::endl;
pcl::visualization::PCLVisualizer viewer("Clouds");
viewer.addCoordinateSystem(1.0,"id",0);
viewer.setSize(1280,720);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr c2_color(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud,*c2_color);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> c2_handler(c2_color,0,255,0);
viewer.addPointCloud(c2_color,c2_handler,"All");
pcl::PointCloud<pcl::PointXYZRGB>::Ptr c1_color(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*plane,*c1_color);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> c1_handler(c1_color,255,0,0);
viewer.addPointCloud(c1_color,c1_handler,"Plane");
float mag3;
Eigen::Vector3f zAxis(0,0,1);
Eigen::Vector3f normal(coefficients->values[0],coefficients->values[1],coefficients->values[2]);
mag3=normal.norm();
std::cout<<normal<<"\n";
normal=(1/mag3)*normal;
float angle=normal.dot(zAxis)/(normal.norm()*zAxis.norm());
viewer.addLine(pcl::PointXYZ(0,0,0),pcl::PointXYZ(normal[0],normal[1],normal[2]),"line",0);
std::cout<<"Angle: "<<acos(angle)*180/M_PI<<"\n";
while(!viewer.wasStopped())
{
viewer.spinOnce();
}
return (0);
}
void OrthogonalCamera::onMouseMove(int x,int y)
{
switch(motionType)
{
case Rotate:
{
int disx=x-oldX;
int disy=y-oldY;
GGL::Point3f zAxis(0,0,1);
float angle=-0.0002f*disx;
GGL::Matrix44f rotation;
rotation.SetRotate(angle,zAxis);
up()=rotation*up();
from()=rotation*(from()-to())+to();
GGL::Point3f roAxis=up()^(from()-to());
angle=-0.0002f*disy;
rotation.SetRotate(angle,roAxis);
from()=rotation*(from()-to())+to();
up()=rotation*up();
light.setPosition(from());
GGL::Point3f dir=from()-to();
light.setDirection(dir);
emit onCameraMotion(this);
}
break;
case Pan:
{
int disx=oldX-x;
int disy=y-oldY;
GGL::Point3f zAxis(0,0,disy*0.002);
GGL::Point3f horizontal=(up()^(from()-to())).Normalize();
horizontal*=disx*0.002;
from()+=horizontal;
to()+=horizontal;
from()+=zAxis;
to()+=zAxis;
emit onCameraMotion(this);
}
break;
case Zoom:
{
int disy=y-oldY;
GGL::Point3f newfrom=from()+((to()-from()).Normalize())*0.01*disy;
scale=-disy*0.0005+scale;
if(scale<0.000001f)
scale=0.000001f;
if((to()-newfrom)*((to()-from()).Normalize())>0.001f)
{
from()=newfrom;
}
else
{
from()=(from()-to()).Normalize()*0.1f+to();
}
emit onCameraMotion(this);
}
break;
}
}
osg::Node* createPreRenderSubGraph(osg::Node* subgraph, unsigned tex_width, unsigned tex_height, osg::Camera::RenderTargetImplementation renderImplementation, bool useImage, bool useTextureRectangle, bool useHDR, osg::Image* image, osg::Camera* camera)
{
if (!subgraph) return 0;
// create a group to contain the flag and the pre rendering camera.
osg::Group* parent = new osg::Group;
// texture to render to and to use for rendering of flag.
osg::Texture* texture = 0;
if (useTextureRectangle)
{
osg::TextureRectangle* textureRect = new osg::TextureRectangle;
textureRect->setTextureSize(tex_width, tex_height);
textureRect->setInternalFormat(GL_RGBA);
textureRect->setFilter(osg::Texture2D::MIN_FILTER,osg::Texture2D::LINEAR);
textureRect->setFilter(osg::Texture2D::MAG_FILTER,osg::Texture2D::LINEAR);
texture = textureRect;
}
else
{
osg::Texture2D* texture2D = new osg::Texture2D;
texture2D->setTextureSize(tex_width, tex_height);
texture2D->setInternalFormat(GL_RGBA);
texture2D->setFilter(osg::Texture2D::MIN_FILTER,osg::Texture2D::LINEAR);
texture2D->setFilter(osg::Texture2D::MAG_FILTER,osg::Texture2D::LINEAR);
texture = texture2D;
}
if (useHDR)
{
texture->setInternalFormat(GL_RGBA16F_ARB);
texture->setSourceFormat(GL_RGBA);
texture->setSourceType(GL_FLOAT);
}
// first create the geometry of the flag of which to view.
{
// XXX ATI linux driver (fglrx) seems buggy in that RTT doesn't work unless this actually happens.
// XXX nvidia linux driver (nvidia 100.14.19 on GeForce 6600/PCI/SSE2 amd64) seems buggy in that RTT is screwed up unless this happens.
// create the to visualize.
osg::Geometry* polyGeom = new osg::Geometry();
polyGeom->setSupportsDisplayList(false);
osg::Vec3 origin(0.0f,0.0f,0.0f);
osg::Vec3 xAxis(1.0f,0.0f,0.0f);
osg::Vec3 yAxis(0.0f,0.0f,1.0f);
osg::Vec3 zAxis(0.0f,-1.0f,0.0f);
float height = 100.0f;
float width = 200.0f;
int noSteps = 20;
osg::Vec3Array* vertices = new osg::Vec3Array;
osg::Vec3 bottom = origin;
osg::Vec3 top = origin; top.z()+= height;
osg::Vec3 dv = xAxis*(width/((float)(noSteps-1)));
osg::Vec2Array* texcoords = new osg::Vec2Array;
// note, when we use TextureRectangle we have to scale the tex coords up to compensate.
osg::Vec2 bottom_texcoord(0.0f,0.0f);
osg::Vec2 top_texcoord(0.0f, useTextureRectangle ? tex_height : 1.0f);
osg::Vec2 dv_texcoord((useTextureRectangle ? tex_width : 1.0f)/(float)(noSteps-1),0.0f);
for(int i=0;i<noSteps;++i)
{
vertices->push_back(top);
vertices->push_back(bottom);
top+=dv;
bottom+=dv;
texcoords->push_back(top_texcoord);
texcoords->push_back(bottom_texcoord);
top_texcoord+=dv_texcoord;
bottom_texcoord+=dv_texcoord;
}
// pass the created vertex array to the points geometry object.
polyGeom->setVertexArray(vertices);
polyGeom->setTexCoordArray(0,texcoords);
osg::Vec4Array* colors = new osg::Vec4Array;
colors->push_back(osg::Vec4(1.0f,1.0f,1.0f,1.0f));
polyGeom->setColorArray(colors);
polyGeom->setColorBinding(osg::Geometry::BIND_OVERALL);
polyGeom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::QUAD_STRIP,0,vertices->size()));
// new we need to add the texture to the Drawable, we do so by creating a
// StateSet to contain the Texture StateAttribute.
osg::StateSet* stateset = new osg::StateSet;
stateset->setTextureAttributeAndModes(0, texture,osg::StateAttribute::ON);
polyGeom->setStateSet(stateset);
polyGeom->setUpdateCallback(new MyGeometryCallback(origin,xAxis,yAxis,zAxis,1.0,1.0/width,0.2f));
osg::Geode* geode = new osg::Geode();
geode->addDrawable(polyGeom);
parent->addChild(geode);
}
// then create the camera node to do the render to texture
{
// set up the background color and clear mask.
camera->setClearColor(osg::Vec4(0.1f,0.1f,0.3f,1.0f));
camera->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
const osg::BoundingSphere& bs = subgraph->getBound();
if (!bs.valid())
{
return subgraph;
}
float znear = 1.0f*bs.radius();
float zfarclip = 3.0f*bs.radius();
// 2:1 aspect ratio as per flag geomtry below.
float proj_top = 0.25f*znear;
float proj_right = 0.5f*znear;
znear *= 0.9f;
zfarclip *= 1.1f;
// default seems to be COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES, which doesn't seem to always work
camera->setComputeNearFarMode(osg::CullSettings::COMPUTE_NEAR_FAR_USING_PRIMITIVES);
camera->setNearFarRatio(0.00001f);
// set up projection.
camera->setProjectionMatrixAsFrustum(-proj_right,proj_right,-proj_top,proj_top,znear,zfarclip);
// set view
camera->setReferenceFrame(osg::Transform::ABSOLUTE_RF);
camera->setViewMatrixAsLookAt(bs.center()-osg::Vec3(0.0f,2.0f,0.0f)*bs.radius(),bs.center(),osg::Vec3(0.0f,0.0f,1.0f));
// set viewport
camera->setViewport(0,0,tex_width,tex_height);
// set the camera to render before the main camera.
camera->setRenderOrder(osg::Camera::PRE_RENDER);
// tell the camera to use OpenGL frame buffer object where supported.
camera->setRenderTargetImplementation(renderImplementation);
if (useImage)
{
// osg::Image* image = new osg::Image;
//image->allocateImage(tex_width, tex_height, 1, GL_RGBA, GL_UNSIGNED_BYTE);
//image->allocateImage(tex_width, tex_height, 1, GL_RGBA, GL_FLOAT);
// attach the image so its copied on each frame.
camera->attach(osg::Camera::COLOR_BUFFER, image);
/*
camera->setPostDrawCallback(new MyCameraPostDrawCallback(image));
// Rather than attach the texture directly to illustrate the texture's ability to
// detect an image update and to subload the image onto the texture. You needn't
// do this when using an Image for copying to, as a seperate camera->attach(..)
// would suffice as well, but we'll do it the long way round here just for demonstation
// purposes (long way round meaning we'll need to copy image to main memory, then
// copy it back to the graphics card to the texture in one frame).
// The long way round allows us to mannually modify the copied image via the callback
// and then let this modified image by reloaded back.
texture->setImage(0, image);
*/
}
else
{
// attach the texture and use it as the color buffer.
camera->attach(osg::Camera::COLOR_BUFFER, texture);
}
// add subgraph to render
camera->addChild(subgraph);
parent->addChild(camera);
}
return parent;
}
ZMPDistributor::ForceTorque ZMPDistributor::distributeZMP(const Eigen::Vector3f& localAnkleLeft,
const Eigen::Vector3f& localAnkleRight,
const Eigen::Matrix4f& leftFootPoseGroundFrame,
const Eigen::Matrix4f& rightFootPoseGroundFrame,
const Eigen::Vector3f& zmpRefGroundFrame,
Bipedal::SupportPhase phase)
{
Eigen::Matrix4f groundPoseLeft = Bipedal::projectPoseToGround(leftFootPoseGroundFrame);
Eigen::Matrix4f groundPoseRight = Bipedal::projectPoseToGround(rightFootPoseGroundFrame);
Eigen::Vector3f localZMPLeft = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseLeft.inverse());
Eigen::Vector3f localZMPRight = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseRight.inverse());
double alpha = computeAlpha(groundPoseLeft, groundPoseRight, zmpRefGroundFrame, localZMPLeft.head(2), localZMPRight.head(2), phase);
//std::cout << "########## " << alpha << " ###########" << std::endl;
ForceTorque ft;
// kg*m/s^2 = N
ft.leftForce = -(1-alpha) * mass * gravity;
ft.rightForce = -alpha * mass * gravity;
// Note we need force as kg*mm/s^2
ft.leftTorque = (localAnkleLeft - localZMPLeft).cross(ft.leftForce * 1000);
ft.rightTorque = (localAnkleRight - localZMPRight).cross(ft.rightForce * 1000);
// ZMP not contained in convex polygone
if (std::fabs(alpha) > std::numeric_limits<float>::epsilon()
&& std::fabs(alpha-1) > std::numeric_limits<float>::epsilon())
{
Eigen::Vector3f leftTorqueGroundFrame = groundPoseLeft.block(0, 0, 3, 3) * ft.leftTorque;
Eigen::Vector3f rightTorqueGroundFrame = groundPoseRight.block(0, 0, 3, 3) * ft.rightTorque;
Eigen::Vector3f tau0 = -1 * (leftTorqueGroundFrame + rightTorqueGroundFrame);
//std::cout << "Tau0World: " << tau0.transpose() << std::endl;
//std::cout << "leftTorqueWorld: " << leftTorqueWorld.transpose() << std::endl;
//std::cout << "rightTorqueWorld: " << rightTorqueWorld.transpose() << std::endl;
// Note: Our coordinate system is different than in the paper!
// Also this is not the same as the ground frame.
Eigen::Vector3f xAxis = leftFootPoseGroundFrame.block(0,3,3,1) + localAnkleLeft
- localAnkleRight - rightFootPoseGroundFrame.block(0,3,3,1);
xAxis /= xAxis.norm();
Eigen::Vector3f zAxis(0, 0, 1);
Eigen::Vector3f yAxis = zAxis.cross(xAxis);
yAxis /= yAxis.norm();
Eigen::Matrix3f centerFrame;
centerFrame.block(0, 0, 3, 1) = xAxis;
centerFrame.block(0, 1, 3, 1) = yAxis;
centerFrame.block(0, 2, 3, 1) = zAxis;
//std::cout << "Center frame:\n" << centerFrame << std::endl;
Eigen::Vector3f centerTau0 = centerFrame.transpose() * tau0;
Eigen::Vector3f leftTorqueCenter;
leftTorqueCenter.x() = (1-alpha)*centerTau0.x();
leftTorqueCenter.y() = centerTau0.y() < 0 ? centerTau0.y() : 0;
leftTorqueCenter.z() = 0;
Eigen::Vector3f rightTorqueCenter;
rightTorqueCenter.x() = alpha*centerTau0.x();
rightTorqueCenter.y() = centerTau0.y() < 0 ? 0 : centerTau0.y();
rightTorqueCenter.z() = 0;
//std::cout << "Tau0Center: " << centerTau0.transpose() << std::endl;
//std::cout << "leftTorqueCenter: " << leftTorqueCenter.transpose() << std::endl;
//std::cout << "rightTorqueCenter: " << rightTorqueCenter.transpose() << std::endl;
// tcp <--- ground frame <--- center frame
ft.leftTorque = groundPoseLeft.block(0, 0, 3, 3).transpose() * centerFrame * leftTorqueCenter;
ft.rightTorque = groundPoseRight.block(0, 0, 3, 3).transpose() * centerFrame * rightTorqueCenter;
}
// Torque depends on timestep, we need a way to do this correctly.
const double torqueFactor = 1;
// convert to Nm
ft.leftTorque *= torqueFactor / 1000.0 / 1000.0;
ft.rightTorque *= torqueFactor / 1000.0 / 1000.0;
//std::cout << "leftTorque: " << ft.leftTorque.transpose() << std::endl;
//std::cout << "rightTorque: " << ft.rightTorque.transpose() << std::endl;
return ft;
}
/**
* Build all the vertices of a quarter dome.
* @param vertBuf Filled with the vertices of the quarter dome.
* @param colorBuf Filled with colors, one for each vertex.
*/
void Sphere::buildQuarterDome(vector<vec3>& vertBuf,
vector<vec4>& colorBuf)
{
vec3 zAxis(0.0f, 0.0f, 1.0f);
vec3 yAxis(0.0f, 1.0f, 0.0f);
vec3 start(0.0f, this->radius, 0.0f);
vector<vector<vec3> > holdVerts;
vector<vec4> colorToggle;
int toggle = 0;
unsigned numTriangleStrips = this->numTriangleStrips / 2;
holdVerts.reserve(numTriangleStrips + 1);
/*
* This works by generating a large triangle composed of numTriangleStrips
* number of triangle strips. A "large triangle" with numTriangleStrips = 2
* would look like this.
*
* /\
* /__\
* /\ /\
* /__\/__\
*
* The vertices are generated by starting at point 0,1,0, and rotating
* 90 degrees around the z axis numTriangleStrips + 1 times. For each
* iteration, vertices are generated by rotating 90 degrees about the y
* axis.
*/
for (unsigned level = 0; level < numTriangleStrips + 1; ++level)
{
float angleZ = half_pi<float>() / numTriangleStrips * level;
mat4 rotationZ = rotate(mat4(1.0f), angleZ, zAxis);
unsigned vertsThisLevel = level + 1;
holdVerts.push_back(vector<vec3>());
holdVerts.back().reserve(vertsThisLevel);
for (unsigned v = 0; v < vertsThisLevel; ++v)
{
float angleY = (vertsThisLevel == 1) ? 0 : half_pi<float>() / (vertsThisLevel - 1) * v;
mat4 rotationY = rotate(mat4(1.0f), angleY, yAxis);
holdVerts.back().push_back(
vec3(rotationY * rotationZ * vec4(start, 1.0f)));
}
}
// Initialize the colors.
colorToggle.reserve(2);
colorToggle.push_back(this->colors.at(0));
colorToggle.push_back(this->colors.at(1));
// Make sure the buffers are empty, and large enough. There will be
// numTriangleStrips ^ 2 * 2 vertices/colors.
colorBuf.clear();
vertBuf.clear();
vertBuf.reserve(numTriangleStrips * numTriangleStrips * 2);
colorBuf.reserve(numTriangleStrips * numTriangleStrips * 2);
/* From the vertices generated above, create a series of triangles that
* can be used to draw the quarter dome.
*
* |Level|Triangles
* |-----|---------
* 0 | 0 |
* / \ | |
* 0---1 | 1 | [0,1,0 UP]
* / \ / \ | |
* 0---1---2 | 2 | [0,1,0 UP] [1,2,1 UP] [0,1,1 DOWN]
* / \ / \ / \ | |
* 0---1---2---3 | 3 | [0,1,0 UP] [1,2,1 UP] [2,3,2 UP] [0,1,1 DOWN] [1,2,2 DOWN]
*
*/
for (unsigned level = 1; level < holdVerts.size(); ++level)
{
unsigned numUp = level;
unsigned numDown = level - 1;
colorBuf.resize(colorBuf.size() + (numUp + numDown) * 3, colorToggle.at(toggle));
toggle ^= 1;
// The vertices are pushed clock-wise for vertex normal computation.
for (unsigned u = 0; u < numUp; ++u)
{
vertBuf.push_back(holdVerts.at(level).at(u));
vertBuf.push_back(holdVerts.at(level - 1).at(u));
vertBuf.push_back(holdVerts.at(level).at(u + 1));
}
for (unsigned d = 0; d < numDown; ++d)
{
vertBuf.push_back(holdVerts.at(level - 1).at(d));
vertBuf.push_back(holdVerts.at(level - 1).at(d + 1));
vertBuf.push_back(holdVerts.at(level).at(d + 1));
}
}
}
/**
* Initialize the Sphere's vertices.
* @param radius The radius of the Sphere.
* @param pProgram The currently installed shader program.
* @param pMatrixStack The World's MatrixStack.
* @name The name of the Sphere.
* @param numTriangleStrips The number of horizontal triangle strips. This
* number should be even. If odd, it is rounded down. The number should
* also be greater than 3. If not, it will be set to 4.
* @param colors Two colors for stripes (see WorldObject).
*/
Sphere::Sphere(float radius, Program* pProgram, MatrixStack* pMatrixStack,
const string& name, unsigned numTriangleStrips, vector<vec4> colors) :
WorldObject(name, pProgram, pMatrixStack)
{
vector<vec3> vertBuf;
vector<vec4> colorBuf;
vector<vec4> colorToggle;
// Basic initialization.
this->radius = radius;
if (numTriangleStrips % 2 == 1)
this->numTriangleStrips = numTriangleStrips - 1;
else
this->numTriangleStrips = numTriangleStrips;
if (numTriangleStrips < 4)
this->numTriangleStrips = 4;
// Make sure there are two colors for striping.
this->setColors(colors, 2);
colorToggle.reserve(2);
colorToggle.push_back(colors.at(0));
colorToggle.push_back(colors.at(1));
// Create a quarter dome.
buildQuarterDome(vertBuf, colorBuf);
this->colors.clear();
/*
* Build the sphere out of 8 quarter domes. The first 4 quarter domes are
* rotated about the Y axis. The second 4 are rotated about the Z axis
* and the Y axis.
*/
// Reserve space for the vertices and colors.
this->vertices.reserve(vertBuf.size() * 8);
this->colors.reserve(colorBuf.size() * 8);
for (int dome = 0; dome < 2; ++dome)
{
vec3 zAxis(0.0f, 0.0f, 1.0f);
mat4 rotationZ = rotate(mat4(1.0f), pi<float>() * dome, zAxis);
for (int i = 0; i < 4; ++i)
{
vec3 yAxis(0.0f, 1.0f, 0.0f);
mat4 rotationY = rotate(mat4(1.0f), half_pi<float>() * i, yAxis);
for (vector<vec3>::iterator it = vertBuf.begin(); it != vertBuf.end(); ++it)
this->vertices.push_back(vec3(rotationZ * rotationY * vec4(*it, 1.0f)));
}
}
// Duplicate the colors. The first 4 quarter domes have identical colors.
for (int i = 0; i < 4; ++i)
copy(colorBuf.begin(), colorBuf.end(), back_inserter(this->colors));
// The bottom 4 quarter domes have inverse colors (otherwise the color at
// the equator would be repeated).
transform(this->colors.begin(),
this->colors.end(),
back_inserter(this->colors),
Inverter<vec4>(colorToggle.at(0), colorToggle.at(1)));
// Compute the normals for each vertex.
this->computeVertexNormals();
// Set up and fill the vertex buffer, color buffer, and normal buffer.
this->getProgram()->createAndFillBuffer(this->vertices,
this->getProgram()->getVertexPositionAttr(), this->getVAO());
this->getProgram()->createAndFillBuffer(this->vertexNormals,
this->getProgram()->getVertexNormalAttr(), this->getVAO());
this->getProgram()->createAndFillBuffer(this->colors,
this->getProgram()->getVertexColorAttr(), this->getVAO());
}
Quaternion::Quaternion(const Matrix4x4& mat)
{
Vector3 xAxis(mat[0]);
Vector3 yAxis(mat[1]);
Vector3 zAxis(mat[2]);
Vector3 scale(xAxis.Magnitude(), yAxis.Magnitude(), zAxis.Magnitude());
// don't use close enough, skip the abs since we're all positive value.
// todo: do we actually care about near-zero?
if (scale.x <= FloatEpsilon || scale.y <= FloatEpsilon || scale.z <= FloatEpsilon)
{
#if defined(XO_SSE)
#else
w = 1.0f;
x = y = z = 0.0f;
#endif
return; // too close.
}
#if defined(XO_SSE)
# if defined(XO_NO_INVERSE_DIVISION)
Vector3 recipScale = Vector3(_mm_div_ps(Vector4::One.xmm, scale.xmm));
# else
Vector3 recipScale = Vector3(_mm_rcp_ps(scale.xmm));
# endif
#else
Vector3 recipScale = Vector3(1.0f / scale.x, 1.0f / scale.y, 1.0f / scale.z);
#endif
xAxis *= recipScale.x;
yAxis *= recipScale.y;
zAxis *= recipScale.z;
// Now calculate the rotation from the resulting matrix (axes).
float trace = xAxis.x + yAxis.y + zAxis.z + 1.0f;
if (trace > 1.0f)
{
float s = 0.5f / Sqrt(trace);
_XO_ASSIGN_QUAT(
0.25f / s,
(yAxis.z - zAxis.y) * s,
(xAxis.y - yAxis.x) * s,
(zAxis.x - xAxis.z) * s);
}
else
{
if (xAxis.x > yAxis.y && xAxis.x > zAxis.z)
{
float s = 0.5f / Sqrt(1.0f + xAxis.x - yAxis.y - zAxis.z);
_XO_ASSIGN_QUAT(
(yAxis.z - zAxis.y) * s,
0.25f / s,
(zAxis.x + xAxis.z) * s,
(yAxis.x + xAxis.y) * s);
}
else if (yAxis.y > zAxis.z)
{
float s = 0.5f / Sqrt(1.0f + yAxis.y - xAxis.x - zAxis.z);
_XO_ASSIGN_QUAT(
(zAxis.x - xAxis.z) * s,
(yAxis.x + xAxis.y) * s,
(zAxis.y + yAxis.z) * s,
0.25f / s);
}
else
{
float s = 0.5f / Sqrt(1.0f + zAxis.z - xAxis.x - yAxis.y);
_XO_ASSIGN_QUAT(
(xAxis.y - yAxis.x) * s,
(zAxis.x + xAxis.z) * s,
0.25f / s,
(zAxis.y + yAxis.z) * s);
}
}
}
/**
* Calulates all the xform matrices
* This is calculated based upon the mAngles in the data structure
*
* I am just rotating around single axis for fingers, not taking abduct into account
* The thumb is a complete fudge.
* Wrist is not being done at all
*/
int GloveData::calcXforms()
{
gmtl::Vec3f xAxis(1.0f, 0.0f, 0.0f);
gmtl::Vec3f yAxis(0.0f, 1.0f, 0.0f);
gmtl::Vec3f zAxis(0.0f, 0.0f, 1.0f);
const float toMeters(1.0f/PositionUnitConversion::ConvertToInches);
gmtl::Vec3f dims[NUM_COMPONENTS][NUM_JOINTS];
// DIJ+1 = Length distal
// DIJ = Length medial
// PIJ = Length proximal
// MPJ = Length to finger
// TODO: Do this once at startup, since it doesn't change.
// And this really should be part of the Glove, so you can specify the dimension of a hand in the config file
dims[THUMB][DIJ+1] = yAxis * (toMeters * 0.5f);
dims[THUMB][DIJ] = yAxis * (toMeters * 0.5f);
dims[THUMB][PIJ] = yAxis * (toMeters * 0.5f);
dims[THUMB][MPJ] = xAxis * (toMeters * -0.5f);
dims[INDEX][DIJ+1] = yAxis * (toMeters * 0.5f);
dims[INDEX][DIJ] = yAxis * (toMeters * 1.0f);
dims[INDEX][PIJ] = yAxis * (toMeters * 1.3f);
dims[INDEX][MPJ] = (yAxis * (toMeters * 1.7f)) + (toMeters * xAxis * -0.4f);
dims[MIDDLE][DIJ+1] = yAxis * (toMeters * 0.5f);
dims[MIDDLE][DIJ] = yAxis * (toMeters * 1.1f);
dims[MIDDLE][PIJ] = yAxis * (toMeters * 1.4f);
dims[MIDDLE][MPJ] = (yAxis * (toMeters * 1.8f)) + (toMeters * xAxis * 0.0f);
dims[RING][DIJ+1] = yAxis * (toMeters * 0.4f);
dims[RING][DIJ] = yAxis * (toMeters * 1.0f);
dims[RING][PIJ] = yAxis * (toMeters * 1.1f);
dims[RING][MPJ] = (yAxis * (toMeters * 1.7f)) + (toMeters * xAxis * 0.4f);
dims[PINKY][DIJ+1] = yAxis * (toMeters * 0.3f);
dims[PINKY][DIJ] = yAxis * (toMeters * 1.0f);
dims[PINKY][PIJ] = yAxis * (toMeters * 0.85f);
dims[PINKY][MPJ] = (yAxis * (toMeters * 1.6f)) + (toMeters * xAxis * 0.7f);
// ----------------------- //
// ----- XFORMS ---------- //
// ----------------------- //
// THUMB
gmtl::setRot(mTransforms[THUMB][DIJ], gmtl::AxisAnglef( mAngles[THUMB][DIJ], xAxis ) );
gmtl::preMult(mTransforms[THUMB][DIJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[THUMB][DIJ]));
gmtl::setRot(mTransforms[THUMB][PIJ], gmtl::AxisAnglef( mAngles[THUMB][PIJ], xAxis ) );
gmtl::preMult(mTransforms[THUMB][PIJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[THUMB][PIJ]));
gmtl::setRot(mTransforms[THUMB][MPJ], gmtl::AxisAnglef( gmtl::Math::PI_OVER_4, zAxis ) );
gmtl::preMult(mTransforms[THUMB][MPJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[THUMB][MPJ]));
// Do we need to rotate this by the mAngles too?
// gmtl::setRot(mTransforms[THUMB][MPJ], gmtl::AxisAnglef( mAngles[THUMB][MPJ], xAxis ) );
// gmtl::preMult(mTransforms[THUMB][MPJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[THUMB][MPJ]));
// INDEX
gmtl::setRot(mTransforms[INDEX][DIJ], gmtl::AxisAnglef( mAngles[INDEX][DIJ], xAxis ) );
gmtl::preMult(mTransforms[INDEX][DIJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[INDEX][DIJ]));
gmtl::setRot(mTransforms[INDEX][PIJ], gmtl::AxisAnglef( mAngles[INDEX][PIJ], xAxis ) );
gmtl::preMult(mTransforms[INDEX][PIJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[INDEX][PIJ]));
gmtl::setRot(mTransforms[INDEX][MPJ], gmtl::AxisAnglef( mAngles[INDEX][MPJ], xAxis ) );
gmtl::preMult(mTransforms[INDEX][MPJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[INDEX][MPJ]));
// MIDDLE
gmtl::setRot(mTransforms[MIDDLE][DIJ], gmtl::AxisAnglef( mAngles[MIDDLE][DIJ], xAxis ) );
gmtl::preMult(mTransforms[MIDDLE][DIJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[MIDDLE][DIJ]));
gmtl::setRot(mTransforms[MIDDLE][PIJ], gmtl::AxisAnglef( mAngles[MIDDLE][PIJ], xAxis ) );
gmtl::preMult(mTransforms[MIDDLE][PIJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[MIDDLE][PIJ]));
gmtl::setRot(mTransforms[MIDDLE][MPJ], gmtl::AxisAnglef( mAngles[MIDDLE][MPJ], xAxis ) );
gmtl::preMult(mTransforms[MIDDLE][MPJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[MIDDLE][MPJ]));
// RING
gmtl::setRot(mTransforms[RING][DIJ], gmtl::AxisAnglef( mAngles[RING][DIJ], xAxis ) );
gmtl::preMult(mTransforms[RING][DIJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[RING][DIJ]));
gmtl::setRot(mTransforms[RING][PIJ], gmtl::AxisAnglef( mAngles[RING][PIJ], xAxis ) );
gmtl::preMult(mTransforms[RING][PIJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[RING][PIJ]));
gmtl::setRot(mTransforms[RING][MPJ], gmtl::AxisAnglef( mAngles[RING][MPJ], xAxis ) );
gmtl::preMult(mTransforms[RING][MPJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[RING][MPJ]));
// PINKY
gmtl::setRot(mTransforms[PINKY][DIJ], gmtl::AxisAnglef( mAngles[PINKY][DIJ], xAxis ) );
gmtl::preMult(mTransforms[PINKY][DIJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[PINKY][DIJ]));
gmtl::setRot(mTransforms[PINKY][PIJ], gmtl::AxisAnglef( mAngles[PINKY][PIJ], xAxis ) );
gmtl::preMult(mTransforms[PINKY][PIJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[PINKY][PIJ]));
gmtl::setRot(mTransforms[PINKY][MPJ], gmtl::AxisAnglef( mAngles[PINKY][MPJ], xAxis ) );
gmtl::preMult(mTransforms[PINKY][MPJ], gmtl::makeTrans<gmtl::Matrix44f>(dims[PINKY][MPJ]));
return 1;
}
