Matrix4 FPSview(Vector3 cameraPosition, float pitch, float yaw)
{
// If the pitch and yaw angles are in degrees,
// they need to be converted to radians. Here
// I assume the values are already converted to radians.
float cosPitch = cos(pitch);
float sinPitch = sin(pitch);
float cosYaw = cos(yaw);
float sinYaw = sin(yaw);
Vector3 xaxis( cosYaw, 0, -sinYaw);
Vector3 yaxis( sinYaw * sinPitch, cosPitch, cosYaw * sinPitch);
Vector3 zaxis( sinYaw * cosPitch, -sinPitch, cosPitch * cosYaw);
// Create a 4x4 view matrix from the right, up, forward and eye position vectors
Matrix4 viewMatrix
(
Vector4( xaxis.x, yaxis.x, zaxis.x, 0 ),
Vector4( xaxis.y, yaxis.y, zaxis.y, 0 ),
Vector4( xaxis.z, yaxis.z, zaxis.z, 0 ),
Vector4(-xaxis.dot(cameraPosition), -yaxis.dot(cameraPosition), -zaxis.dot(cameraPosition), 1 )
);
return viewMatrix;
}
S2Matrix4x4 Camera::GetViewMatrix() const {
const Transform &trans = *GetTransform();
//Use spherical coordinate for forward vector.
const S2Vector3 & rotate = trans.GetRotate();
float theta = rotate[0], phi = rotate[1];
phi = phi > 1.57f ? 1.57f : phi;
phi = phi < -1.57f ? -1.57f : phi;
S2Vector3 zaxis(sin(theta)*cos(phi), sin(phi), cos(theta)*cos(phi));
S2Vector3 xaxis = S2Vector3(0.0f, 1.0f, 0.0f).Cross(zaxis);
xaxis.Normalize();
S2Vector3 yaxis = zaxis.Cross(xaxis);
S2Vector3 position = trans.GetTranslate();
float p_dot_x = position.Dot(xaxis);
float p_dot_y = position.Dot(yaxis);
float p_dot_z = position.Dot(zaxis);
return S2Matrix4x4(
xaxis[0], xaxis[1], xaxis[2], -p_dot_x,
yaxis[0], yaxis[1], yaxis[2], -p_dot_y,
zaxis[0], zaxis[1], zaxis[2], -p_dot_z,
0.0f, 0.0f, 0.0f, 1.0f
);
}
void
avtExtrudeFilter::SetAtts(const AttributeGroup *a)
{
atts = *(const ExtrudeAttributes*)a;
avtVector axis(atts.GetAxis()), zaxis(0.,0.,1.);
if ((axis.x == 0. && axis.y == 0. && axis.z == 0.)
#if 0
|| zaxis.dot(axis) == 0.
#endif
)
{
EXCEPTION1(BadVectorException, "Extrusion Axis");
}
if(atts.GetLength() <= 0.)
{
EXCEPTION1(ImproperUseException, "Length must be greater than 0.");
}
if(atts.GetSteps() < 1)
{
EXCEPTION1(ImproperUseException, "Steps must be at least 1.");
}
}
void LLCamera::calculateWorldFrustumPlanes()
{
F32 d;
LLVector3 center = mOrigin - mXAxis*mNearPlane;
mWorldPlanePos = center;
for (int p=0; p<4; p++)
{
LLVector3 pnorm = LLVector3(mLocalPlanes[p]);
LLVector3 norm = rotateToAbsolute(pnorm);
norm.normVec();
d = -(center * norm);
mWorldPlanes[p] = LLPlane(norm, d);
}
// horizontal planes, perpindicular to (0,0,1);
LLVector3 zaxis(0, 0, 1.0f);
F32 yaw = getYaw();
{
LLVector3 tnorm = LLVector3(mLocalPlanes[PLANE_LEFT]);
tnorm.rotVec(yaw, zaxis);
d = -(mOrigin * tnorm);
mHorizPlanes[HORIZ_PLANE_LEFT] = LLPlane(tnorm, d);
}
{
LLVector3 tnorm = LLVector3(mLocalPlanes[PLANE_RIGHT]);
tnorm.rotVec(yaw, zaxis);
d = -(mOrigin * tnorm);
mHorizPlanes[HORIZ_PLANE_RIGHT] = LLPlane(tnorm, d);
}
}
void CBillBoard::onRender(const D3DXMATRIX& mView, const D3DXVECTOR3& vPos) {
CDirect3D::getInstance()->AlphaTestEnable(TRUE);
CDirect3D::getInstance()->AlphaFunction(D3DCMP_GREATER);
CDirect3D::getInstance()->AlphaReference(0x80);
CDirect3D::getInstance()->SetD3DFVF(SVertexT::FVF);
CDirect3D::getInstance()->SetStreamSource(0, m_pVB, 0, sizeof(SVertexT));
CDirect3D::getInstance()->SetTexture(0, m_pTexture);
D3DXVECTOR3 vUp(mView._12, mView._22, mView._32);
/* simpler, yet supposed-to-be less efficient *
D3DXMatrixLookAtRH(&m_matWorld, &m_vPos, &vPos, &vUp); // Backward Direction
m_matWorld._41 = m_matWorld._42 = m_matWorld._43 = 0.0f;
D3DXMatrixTranspose(&m_matWorld, &m_matWorld);
/* optimized by doing manually */
D3DXVECTOR3 zaxis(m_vPos - vPos); D3DXVec3Normalize(&zaxis, &zaxis);
D3DXVECTOR3 xaxis; D3DXVec3Normalize(D3DXVec3Cross(&xaxis, &vUp, &zaxis), &xaxis);
D3DXVECTOR3 yaxis; D3DXVec3Cross(&yaxis, &zaxis, &xaxis);
m_matWorld._11 = xaxis.x; m_matWorld._12 = xaxis.y; m_matWorld._13 = xaxis.z;
m_matWorld._21 = yaxis.x; m_matWorld._22 = yaxis.y; m_matWorld._23 = yaxis.z;
m_matWorld._31 = zaxis.x; m_matWorld._32 = zaxis.y; m_matWorld._33 = zaxis.z;
/**/
m_matWorld._41 = m_vPos.x; m_matWorld._42 = m_vPos.y; m_matWorld._43 = m_vPos.z;
CDirect3D::getInstance()->SetTransform(D3DTS_WORLD, &m_matWorld);
CDirect3D::getInstance()->DrawPrimitive(D3DPT_TRIANGLESTRIP, 0, 2);
CDirect3D::getInstance()->SetTexture(0, NULL);
CDirect3D::getInstance()->AlphaTestEnable(FALSE);
}
void
StarRegionScalarInteractor::updateScale()
{
osg::Matrix scaleMat, rotMat, offsetMat;
scaleMat.makeScale (interScale/cover->getScale(), interScale/cover->getScale(), interScale/cover->getScale());
osg::Vec3 n;
n = localMat * interNormal;
rotMat.makeRotate (defAxis, n);
scaleMat.makeScale (interScale/cover->getScale(), interScale/cover->getScale(), interScale/cover->getScale());
offsetMat.mult (scaleMat, rotMat);
osg::Vec3 p;
p=interPos;
float dr=interScale/cover->getScale();
//p[2]+= -regionRadius - dr -4*dindex*dr;
osg::Vec3 zaxis (0,0,1);
zaxis = localMat * zaxis;
p+=zaxis* (-regionRadius - dr -4*dindex*dr);
/* offsetMat.setRow(3, p); */
for (int i = 0; i < 3; i++)
{
offsetMat (3, i) = p[i];
}
worldDCS->setMatrix (offsetMat);
}
void rgl_bbox(int* successptr,
int* idata,
double* ddata,
double* xat, char** xtext,
double* yat, char** ytext,
double* zat, char** ztext)
{
int success = RGL_FAIL;
Device* device = deviceManager->getAnyDevice();
if (device) {
int xticks = idata[0];
int yticks = idata[1];
int zticks = idata[2];
int xlen = idata[3];
int ylen = idata[4];
int zlen = idata[5];
int marklen_rel = idata[6];
float xunit = (float) ddata[0];
float yunit = (float) ddata[1];
float zunit = (float) ddata[2];
float marklen = (float) ddata[3];
AxisInfo xaxis(xticks, xat, xtext, xlen, xunit);
AxisInfo yaxis(yticks, yat, ytext, ylen, yunit);
AxisInfo zaxis(zticks, zat, ztext, zlen, zunit);
success = as_success( device->add( new BBoxDeco(currentMaterial, xaxis, yaxis, zaxis, marklen, (bool) marklen_rel ) ) );
}
*successptr = success;
}
ofVec3f GrainViewTransform(ofVec3f InPutPoint, ofVec3f Eye, ofVec3f Target, ofVec3f UpVec){
// Transform the cordinates of a point InPutPoint. For a camera placed at Eye and looking at target with Up vector UpVec
ofMatrix4x4 ViewMat;
ofVec3f zaxis(Target - Eye);
zaxis.normalize();
ofVec3f xaxis = UpVec.getCrossed(zaxis);
xaxis.normalize();
ofVec3f yaxis = zaxis.getCrossed(xaxis);
// translate vect
InPutPoint -= Eye;
// project on each axis
ofVec3f Outpos;
Outpos.x = -xaxis.dot(InPutPoint);
Outpos.y = -yaxis.dot(InPutPoint);
Outpos.z = zaxis.dot(InPutPoint);
return Outpos;
}
Camera & Camera::TranslateForward(float distance) {
Transform &trans = *GetTransform();
//Use spherical coordinate for forward vector.
const S2Vector3 & rotate = trans.GetRotate();
float theta = rotate[0], phi = rotate[1];
phi = phi > 1.57f ? 1.57f : phi;
phi = phi < -1.57f ? -1.57f : phi;
S2Vector3 zaxis(sin(theta)*cos(phi), sin(phi), cos(theta)*cos(phi));
trans.Translate(zaxis * distance);
return *this;
}
AtomStickInteractor::AtomStickInteractor(string symbol, const char *interactorName, Atom *myAtom, osg::Matrix initialOrientation, osg::Vec3 initialPos, osg::Vec3 stickDir, float size, osg::Vec4 color)
: coVR3DRotCenterInteractor(initialOrientation, initialPos, size, coInteraction::ButtonA, "Hand", interactorName, coInteraction::Medium)
{
if (opencover::cover->debugLevel(3))
fprintf(stderr, "AtomStickInteractor::AtomStickInteractor\n");
initialOrientation_ = initialOrientation;
initialPos_ = initialPos;
dir_ = stickDir;
dir_.normalize();
symbol_ = symbol;
color_ = color;
myAtom_ = myAtom;
osg::Vec3 zaxis(0, 0, 1);
osg::Matrix mr;
mr.makeRotate(zaxis, dir_);
mr.postMultTranslate(dir_ * 0.5 * _interSize);
// cylinder of lengths size, beginning at center of sphere,
//sphere is 0,5*size so also 0.5*size of the cylinder comes out of the spehere
osg::Cylinder *myCylinder = new osg::Cylinder(osg::Vec3(0, 0, 0), 0.2 * _interSize, _interSize);
osg::TessellationHints *hint = new osg::TessellationHints();
hint->setDetailRatio(0.5);
osg::ShapeDrawable *cylinderDrawable = new osg::ShapeDrawable(myCylinder, hint);
cylinderDrawable->setColor(osg::Vec4(color[0], color[1], color[2], color[3]));
osg::Geode *cylinderGeode = new osg::Geode();
osg::StateSet *normalState = opencover::VRSceneGraph::instance()->loadDefaultGeostate(osg::Material::AMBIENT_AND_DIFFUSE);
cylinderGeode->setStateSet(normalState);
transform_ = new osg::MatrixTransform();
transform_->setMatrix(mr);
cylinderGeode->addDrawable(cylinderDrawable);
transform_->addChild(cylinderGeode);
//coVRIntersectionInteractor::geometryNode
geometryNode = transform_;
// remove old geometry
scaleTransform->removeChild(0, scaleTransform->getNumChildren());
// add geometry to node and remove old scaling
scaleTransform->addChild(transform_);
osg::Matrix s;
scaleTransform->setMatrix(s);
connectedStick_ = NULL; //not connected
oldInvMat_.invert(getMatrix());
}
inline void FXGLVertices::renderLines(FXGLViewer *viewer, bool isHit, bool complex){
#ifdef HAVE_GL_H
FXGLColor col(color);
GLUquadricObj* quad=0;
if(complex){
quad=gluNewQuadric();
gluQuadricDrawStyle(quad,(GLenum)GLU_FILL);
}
FXuint inc=(!(options & (SHADING_SMOOTH|SHADING_FLAT)) && viewer->doesTurbo()) ? 4 : 1;
for(FXuint n=0; n<vertexNumber-complex; n+=inc){
if(!isHit){
if(colorGenerator) for(FXuint c=0; c<inc; c++) colorGenerator(col, this, viewer, n, colorGeneratorData);
if(complex && (options & (SHADING_SMOOTH|SHADING_FLAT))){
glMaterialfv(GL_FRONT_AND_BACK,GL_DIFFUSE,&col.r);
}
else
glColor4fv(&col.r);
}
if(complex){
FXVec3f vector=vertices[n+1]-vertices[n];
FXfloat len=vector.length();
if(!(options & VERTICES_POINTS) || len>=pointSize*0.8f){
glPushMatrix();
glTranslatef(vertices[n].x,vertices[n].y,vertices[n].z);
FXVec3f zaxis(0.0f,0.0f,vector.z<0 ? 1.0f : -1.0f);
// Cylinder points in Z direction, so we need to rotate so it follows vector
FXVec3f vectornormal(vecnormalize(vector));
FXVec3f axis(vectornormal^zaxis);
FXfloat angle=((FXfloat) RTOD)*asinf(axis.length());
//FXfloat dotproduct=vectornormal*axis;
if(vector.z<0) angle+=180;
glRotatef(angle,axis.x,axis.y,axis.z);
gluCylinder(quad,lineSize/2,lineSize/2,len,8,8);
glPopMatrix();
}
}
else{
glVertex3fv(&vertices[n].x);
}
}
if(complex){
gluDeleteQuadric(quad);
}
#endif
}
Matrix4x4 Matrix4x4::LookTo(const Vector3 &eye_position, const Vector3 &to_direction, const Vector3 &up_direction)
{
Matrix4x4 m = Matrix4x4::Identity();
Vector3 zaxis(-to_direction);
zaxis.Normalize();
Vector3 xaxis = zaxis * up_direction;
xaxis.Normalize();
Vector3 yaxis = xaxis * zaxis;
m.m00 = xaxis.x; m.m01 = xaxis.y; m.m02 = xaxis.z; m.m03 = -xaxis.Dot(eye_position);
m.m10 = yaxis.x; m.m11 = yaxis.y; m.m12 = yaxis.z; m.m13 = -yaxis.Dot(eye_position);
m.m20 = zaxis.x; m.m21 = zaxis.y; m.m22 = zaxis.z; m.m23 = -zaxis.Dot(eye_position);
m.m30 = 0; m.m31 = 0; m.m32 = 0; m.m33 = 1.0f;
return m;
}
int MyCRCLServer::HandleMoveToType(MoveToType *cmd)
{
double x, y, z;
double xi, xj, xk;
double zi, zj, zk;
double r, p, w;
x = cmd->EndPosition->Point->X->val;
y = cmd->EndPosition->Point->Y->val;
z = cmd->EndPosition->Point->Z->val;
xi = cmd->EndPosition->XAxis->I->val;
xj = cmd->EndPosition->XAxis->J->val;
xk = cmd->EndPosition->XAxis->K->val;
tf::Vector3 xaxis(xi, xj, xk);
zi = cmd->EndPosition->ZAxis->I->val;
zj = cmd->EndPosition->ZAxis->J->val;
zk = cmd->EndPosition->ZAxis->K->val;
tf::Vector3 zaxis(zi, zj, zk);
tf::Vector3 yaxis = zaxis.cross(xaxis);
tf::Matrix3x3 m(xi, yaxis.getX(), zi,
xj, yaxis.getY(), zj,
xk, yaxis.getZ(), zk);
m.getRPY(r, p, w);
status.setXYZ(x, y, z);
status.setVec(xi, xj, xk, zi, zj, zk);
status.setJointPosition(1, x);
status.setJointPosition(2, y);
status.setJointPosition(3, z);
status.setJointPosition(4, r);
status.setJointPosition(5, p);
status.setJointPosition(6, w);
printf("MoveToType(%d) %f %f %f / %f %f %f\n",
cmd->CommandID->val,
x, y, z, r, p, w);
return 0;
}
void osgParticleHPS::ParticleTrail::renderDebug( float scale ) const
{
osg::Matrix rotMtx;
osg::Vec3 ptA, ptB;
osg::Vec3 xaxis( 1., 0., 0. );
osg::Vec3 yaxis( 0., 1., 0. );
osg::Vec3 zaxis( 0., 0., 1. );
glBegin( GL_LINES );
int i;
int numPoints = positions_.size();
for( i = 0; i < numPoints; i++ )
{
const HPSTrailPoint &point = positions_[i];
float sTexCoord = (float)i / (float)numPoints;
rotMtx.makeRotate(
point.rpy[0], yaxis,
point.rpy[1], xaxis,
point.rpy[2], zaxis
);
ptA = osg::Vec3(scale, 0., 0.) * rotMtx;
ptA += point.pos;
glVertex3fv( point.pos.ptr() );
glVertex3fv( ptA.ptr() );
ptA = osg::Vec3(0., 0., scale) * rotMtx;
ptA += point.pos;
glVertex3fv( point.pos.ptr() );
glVertex3fv( ptA.ptr() );
}
glEnd();
}
void osgParticleHPS::ParticleTrail::renderCrossRibbon() const
{
osg::Matrix rotMtx;
osg::Vec3 ptA, ptB;
osg::Vec3 xaxis( 1., 0., 0. );
osg::Vec3 yaxis( 0., 1., 0. );
osg::Vec3 zaxis( 0., 0., 1. );
int numPoints = positions_.size();
float t = 0.;
float tIncrement = 1. / maxNumPointsToRecord_;
const osgParticleHPS::rangev4 &colorRange = getColorRange();
const osgParticleHPS::Interpolator *colorInterp = getColorInterpolator();
const osgParticleHPS::rangev3 &sizeRange = getSizeRange();
const osgParticleHPS::Interpolator *sizeInterp = getSizeInterpolator();
// This offset will give the ribbon texture a much smoother feel.
// Without it, the texture would "pulse" and jump as the points at the
// tail end of the ribbon are removed.
float sTexCoordOffset = 1. / (float)numPoints;
sTexCoordOffset *= (t0_ - timestampForLastNewPoint_) / timeBetweenPoints_;
glBegin( GL_QUAD_STRIP );
int i;
for( i = 0; i < numPoints; i++ )
{
const HPSTrailPoint &point = positions_[i];
osg::Vec4 interpColor = colorInterp->interpolate( t, colorRange );
osg::Vec3 thisPosSize = sizeInterp->interpolate( t, sizeRange );
t += tIncrement;
float sTexCoord;
if( stretchTexture )
{
sTexCoord = (float)i / (float)numPoints;
if( i > 0 )
sTexCoord += sTexCoordOffset;
}
else
{
if( totalHistoryPoints%2 )
sTexCoord = (i%2) ? 0.0 : 1.0;
else
sTexCoord = (i%2) ? 1.0 : 0.0;
}
rotMtx.makeRotate(
point.rpy[0], yaxis,
point.rpy[1], xaxis,
point.rpy[2], zaxis
);
ptA = osg::Vec3(thisPosSize.x(), 0., 0.) * rotMtx;
ptB = ptA * -1.;
ptA += point.pos;
ptB += point.pos;
glColor4f( interpColor[0], interpColor[1], interpColor[2], interpColor[3] );
glTexCoord2f( sTexCoord, 1. );
glVertex3fv( ptA.ptr() );
glTexCoord2f( sTexCoord, 0. );
glVertex3fv( ptB.ptr() );
}
glEnd();
t = 0.0;
glBegin( GL_QUAD_STRIP );
for( i = 0; i < numPoints; i++ )
{
const HPSTrailPoint &point = positions_[i];
osg::Vec4 interpColor = colorInterp->interpolate( t, colorRange );
osg::Vec3 thisPosSize = sizeInterp->interpolate( t, sizeRange );
t += tIncrement;
float sTexCoord;
if( stretchTexture )
{
sTexCoord = (float)i / (float)numPoints;
if( i > 0 )
sTexCoord += sTexCoordOffset;
}
else
{
if( totalHistoryPoints%2 )
sTexCoord = (i%2) ? 0.0 : 1.0;
else
sTexCoord = (i%2) ? 1.0 : 0.0;
}
rotMtx.makeRotate(
point.rpy[0], yaxis,
point.rpy[1], xaxis,
point.rpy[2], zaxis
);
ptA = osg::Vec3(0., 0., thisPosSize.z()) * rotMtx;
ptB = ptA * -1.;
ptA += point.pos;
ptB += point.pos;
glColor4f( interpColor[0], interpColor[1], interpColor[2], interpColor[3] );
glTexCoord2f( sTexCoord, 1. );
glVertex3fv( ptA.ptr() );
glTexCoord2f( sTexCoord, 0. );
glVertex3fv( ptB.ptr() );
}
glEnd();
}
//--------------------------------------------------------------
// render the planet
void Planet::render(
int graphicsWidth,
int graphicsHeight,
double angle)
{
// if shader didn't compile, nothing we can do
if (m_planetShader == 0)
return;
double atmos = 20.0;
double radius = 1200.0;
double eyeDist = m_eyePt.length()/radius;
double a = 1.0;
if (eyeDist < a)
return; // below surface, nothing to do
double b = sqrt(eyeDist*eyeDist - a*a);
double h = (a*b)/eyeDist;
double m = (a*a)/eyeDist;
h += atmos/radius;
// x axis from planet center towards eye
mgPoint3 xaxis(m_eyePt);
xaxis.normalize();
// build y axis
mgPoint3 yaxis(xaxis);
yaxis.cross(mgPoint3(0.0, 1.0, 0.0));
yaxis.normalize();
mgPoint3 zaxis(yaxis);
zaxis.cross(xaxis);
zaxis.normalize();
mgMatrix4 transform;
transform._11 = xaxis.x;
transform._12 = xaxis.y;
transform._13 = xaxis.z;
transform._21 = yaxis.x;
transform._22 = yaxis.y;
transform._23 = yaxis.z;
transform._31 = zaxis.x;
transform._32 = zaxis.y;
transform._33 = zaxis.z;
VertexPlanet tl, tr, bl, br;
mgPoint3 pt;
transform.mapPt(m, -h, h, pt.x, pt.y, pt.z);
tl.setPoint(radius*pt.x, radius*pt.y, radius*pt.z);
transform.mapPt(m, h, h, pt.x, pt.y, pt.z);
tr.setPoint(radius*pt.x, radius*pt.y, radius*pt.z);
transform.mapPt(m, -h, -h, pt.x, pt.y, pt.z);
bl.setPoint(radius*pt.x, radius*pt.y, radius*pt.z);
transform.mapPt(m, h, -h, pt.x, pt.y, pt.z);
br.setPoint(radius*pt.x, radius*pt.y, radius*pt.z);
// inverse of world transform
mgMatrix4 model;
model.rotateYDeg(-angle);
mgPoint3 lightDir(1.0, 0.25, 0.0);
lightDir.normalize();
mgPoint3 modelLightDir;
model.mapPt(lightDir, modelLightDir);
transform.multiply(model);
mgPoint3 modelEye;
transform.mapPt(eyeDist, 0.0, 0.0, modelEye.x, modelEye.y, modelEye.z);
transform.mapPt(m, -h, h, pt.x, pt.y, pt.z);
tl.setModelPoint(pt.x, pt.y, pt.z);
transform.mapPt(m, h, h, pt.x, pt.y, pt.z);
tr.setModelPoint(pt.x, pt.y, pt.z);
transform.mapPt(m, -h, -h, pt.x, pt.y, pt.z);
bl.setModelPoint(pt.x, pt.y, pt.z);
transform.mapPt(m, h, -h, pt.x, pt.y, pt.z);
br.setModelPoint(pt.x, pt.y, pt.z);
mgMatrix4 viewMatrix;
viewMatrix.translate(-m_eyePt.x, -m_eyePt.y, -m_eyePt.z);
viewMatrix.multiply(m_eyeMatrix);
mgMatrix4 worldProjection;
createProjection(worldProjection, graphicsWidth, graphicsHeight);
mgMatrix4 mvpMatrix(viewMatrix);
mvpMatrix.multiply(worldProjection);
setupShader(mvpMatrix, modelEye, modelLightDir);
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_CUBE_MAP, m_surfaceTexture);
glActiveTexture(GL_TEXTURE1);
glBindTexture(GL_TEXTURE_CUBE_MAP, m_cloudsTexture);
glBindVertexArray(m_vertexArray);
glBindBuffer(GL_ARRAY_BUFFER, m_vertexBuffer);
VertexPlanet data[6];
data[0] = tl;
data[1] = tr;
data[2] = bl;
data[3] = bl;
data[4] = tr;
data[5] = br;
int vertexSize = sizeof(VertexPlanet);
int count = 6;
glBufferData(GL_ARRAY_BUFFER, vertexSize * count, data, GL_DYNAMIC_DRAW);
glDrawArrays(GL_TRIANGLES, 0, count);
glBindBuffer(GL_ARRAY_BUFFER, 0);
glBindVertexArray(0);
glActiveTexture(GL_TEXTURE0);
}
coVR1DTransInteractor::coVR1DTransInteractor(pfVec3 o, pfVec3 d, float min, float max, float val, float size, int showConstr, char *name)
{
pfMatrix wm, tm, sm;
pfVec3 zaxis(0,0,1);
origin = o;
dir = d;
dir.normalize();
minVal = min;
maxVal = max;
currVal = val;
interSize = size;
interactorName= new char[strlen(name)+1];
strcpy(interactorName, name);
if ((showConstr == SHOW_CONSTRAINTS_ALWAYS)
|| (showConstr == SHOW_CONSTRAINTS_ONTOUCH)
|| (showConstr == SHOW_CONSTRAINTS_ONSELECT))
showConstraints = showConstr;
else
{
fprintf(stderr,"\nERROR in coVR1DTransInteractor::coVR1DTransInteractor\n");
fprintf(stderr,"\tillegal value [%d] for showConstraints\n", showConstr);
showConstraints = SHOW_CONSTRAINTS_ALWAYS;
}
float interScale = size/cover->getScale();
fprintf(stderr,"interactor size=%f cover scale = %f\n", interSize, cover->getScale());
// initialize flags
interactionOngoing = false;
isI = false;
isS = false;
isE = false;
justHit = true;
// get debug level
const char *line = CoviseConfig::getEntry("COVERConfig.DEBUG_LEVEL");
if (line)
sscanf(line,"%d", &debugLevel);
else
debugLevel = 0;
// debugprint
if (debugLevel>0)
{
fprintf(stderr,"\ncoVR1DTransInteractor:coVR1DTransInteractor [%s]\n", interactorName);
fprintf(stderr,"\tshowConstraints=[%d]\n", showConstraints);
fprintf(stderr,"\torigin=[%f %f %f]\n", origin[0], origin[1], origin[2]);
fprintf(stderr,"\tdir=[%f %f %f]\n", dir[0], dir[1], dir[2]);
fprintf(stderr,"\tminVal=[%f] maxVal=[%f] currVal=[%f]\n", minVal, maxVal, currVal);
}
// check and adjust the ranges
checkRange();
// load geostates only once
geostate = loadDefaultGeostate();
unlightedGeostate = loadUnlightedGeostate();
// objectsRoot
// |
// worldDCS
// |
// interactorRoot
// | |
// lineGeode sphereTransDCS
// |
// sphereScaleDCS
// |
// sphereGeode
if(debugLevel>1)
fprintf(stderr,"\tcreating the scenegraph\n");
objectsRoot = cover->getObjectsRoot();
worldDCS = new pfDCS();
wm.makeVecRotVec(zaxis, dir);
wm[3][0] = origin[0];
wm[3][1] = origin[1];
wm[3][2] = origin[2];
worldDCS->setMat(wm);
interactorRoot = new pfGroup();
sphereTransDCS = new pfDCS();
tm.makeTrans(0, 0, currVal);
sphereTransDCS->setMat(tm);
sphereScaleDCS = new pfDCS();
sm.makeScale(interScale, interScale, interScale);
sphereScaleDCS->setMat(sm);
sphereGeode = createSphere();
lineGeode = createLine();
objectsRoot->addChild(worldDCS);
worldDCS->addChild(interactorRoot);
interactorRoot->addChild(lineGeode);
interactorRoot->addChild(sphereTransDCS);
sphereTransDCS->addChild(sphereScaleDCS);
sphereScaleDCS->addChild(sphereGeode);
if (showConstraints == SHOW_CONSTRAINTS_ALWAYS)
showLine();
else
hideLine();
if(debugLevel>1)
fprintf(stderr,"\tcreating the highlights\n");
// highlights
isectedHl = new pfHighlight();
isectedHl->setMode(PFHL_FILL);
isectedHl->setColor(PFHL_FGCOLOR, 0.6, 0.6, 0.0);
isectedHl->setLineWidth(3.0);
selectedHl = new pfHighlight();
selectedHl->setMode(PFHL_FILL);
selectedHl->setColor(PFHL_FGCOLOR, 0.0, 0.6, 0.0);
selectedHl->setLineWidth(3.0);
}
//--------------------------------------------------------------
// get avatar orientation under coordinate system
void SolarSystem::getCoordBasis(
int coordSystem,
mgMatrix4& basis)
{
// get position of moon
double angle = (2*PI*m_moonMonth)/360;
mgPoint3 moonCenter(MOON_DISTANCE*sin(angle), 0, MOON_DISTANCE*cos(angle));
// get coordinate system based on object
mgPoint3 xaxis(1.0, 0.0, 0.0);
mgPoint3 yaxis(0.0, 1.0, 0.0);
mgPoint3 zaxis(0.0, 0.0, 1.0);
switch (coordSystem)
{
case COORDS_PLANET:
// y axis points away from center of planet
yaxis = m_coordPosn;
yaxis.normalize();
// construct z axis orthonal to y
zaxis = yaxis;
zaxis.cross(mgPoint3(0.0, 1.0, 0.0));
zaxis.normalize();
// construct x axis
xaxis = yaxis;
xaxis.cross(zaxis);
xaxis.normalize();
break;
case COORDS_MOON:
// y axis points away from center of moon
yaxis = m_coordPosn;
yaxis.normalize();
// construct z axis orthonal to y
zaxis = yaxis;
zaxis.cross(mgPoint3(0.0, 1.0, 0.0));
zaxis.normalize();
// construct x axis
xaxis = yaxis;
xaxis.cross(zaxis);
xaxis.normalize();
break;
case COORDS_RING:
// y axis points towards center of ring
yaxis.x = -m_coordPosn.x;
yaxis.z = -m_coordPosn.z;
yaxis.y = 0.0;
yaxis.normalize();
// x axis is across the ring
xaxis.x = 0.0; xaxis.y = 1.0; xaxis.z = 0.0;
// construct z axis
zaxis = xaxis;
zaxis.cross(yaxis);
zaxis.normalize();
break;
case COORDS_BELT:
break;
case COORDS_SPACE:
// use default axis
break;
}
// apply object surface orientation
basis._11 = xaxis.x;
basis._21 = xaxis.y;
basis._31 = xaxis.z;
basis._12 = yaxis.x;
basis._22 = yaxis.y;
basis._32 = yaxis.z;
basis._13 = zaxis.x;
basis._23 = zaxis.y;
basis._33 = zaxis.z;
}
bool OBBRayPicking::testRayOBBIntersection(
glm::vec3 ray_origin, // Ray origin, in world space
glm::vec3 ray_direction, // Ray direction (NOT target position!), in world space. Must be normalize()'d.
glm::vec3 aabb_min, // Minimum X,Y,Z coords of the mesh when not transformed at all.
glm::vec3 aabb_max, // Maximum X,Y,Z coords. Often aabb_min*-1 if your mesh is centered, but it's not always the case.
const glm::mat4 &ModelMatrix, // Transformation applied to the mesh (which will thus be also applied to its bounding box)
const glm::vec3 &position, // the position because model matrix doesnt do anything in this game "right now"
float &intersection_distance // Output : distance between ray_origin and the intersection with the OBB
){
// Intersection method from Real-Time Rendering and Essential Mathematics for Games
float tMin = 0.0f;
float tMax = 100000.0f;
glm::vec3 OBBposition_worldspace(ModelMatrix[3].x * position.x, ModelMatrix[3].y * position.y, ModelMatrix[3].z * position.z);
glm::vec3 delta = OBBposition_worldspace - ray_origin;
// Test intersection with the 2 planes perpendicular to the OBB's X axis
{
glm::vec3 xaxis(ModelMatrix[0].x, ModelMatrix[0].y, ModelMatrix[0].z);
float e = glm::dot(xaxis, delta);
float f = glm::dot(ray_direction, xaxis);
if (fabs(f) > 0.001f)
{ // Standard case
float t1 = (e + aabb_min.x) / f; // Intersection with the "left" plane
float t2 = (e + aabb_max.x) / f; // Intersection with the "right" plane
// t1 and t2 now contain distances betwen ray origin and ray-plane intersections
// We want t1 to represent the nearest intersection,
// so if it's not the case, invert t1 and t2
if (t1>t2){
float w = t1; t1 = t2; t2 = w; // swap t1 and t2
}
// tMax is the nearest "far" intersection (amongst the X,Y and Z planes pairs)
if (t2 < tMax)
tMax = t2;
// tMin is the farthest "near" intersection (amongst the X,Y and Z planes pairs)
if (t1 > tMin)
tMin = t1;
// And here's the trick :
// If "far" is closer than "near", then there is NO intersection.
// See the images in the tutorials for the visual explanation.
if (tMax < tMin)
return false;
}
else
{ // Rare case : the ray is almost parallel to the planes, so they don't have any "intersection"
if (-e + aabb_min.x > 0.0f || -e + aabb_max.x < 0.0f)
return false;
}
}
// Test intersection with the 2 planes perpendicular to the OBB's Y axis
// Exactly the same thing than above.
{
glm::vec3 yaxis(ModelMatrix[1].x, ModelMatrix[1].y, ModelMatrix[1].z);
float e = glm::dot(yaxis, delta);
float f = glm::dot(ray_direction, yaxis);
if (fabs(f) > 0.001f){
float t1 = (e + aabb_min.y) / f;
float t2 = (e + aabb_max.y) / f;
if (t1>t2){ float w = t1; t1 = t2; t2 = w; }
if (t2 < tMax)
tMax = t2;
if (t1 > tMin)
tMin = t1;
if (tMin > tMax)
return false;
}
else{
if (-e + aabb_min.y > 0.0f || -e + aabb_max.y < 0.0f)
return false;
}
}
// Test intersection with the 2 planes perpendicular to the OBB's Z axis
// Exactly the same thing than above.
{
glm::vec3 zaxis(ModelMatrix[2].x, ModelMatrix[2].y, ModelMatrix[2].z);
float e = glm::dot(zaxis, delta);
float f = glm::dot(ray_direction, zaxis);
if (fabs(f) > 0.001f){
float t1 = (e + aabb_min.z) / f;
float t2 = (e + aabb_max.z) / f;
if (t1>t2){ float w = t1; t1 = t2; t2 = w; }
if (t2 < tMax)
tMax = t2;
if (t1 > tMin)
tMin = t1;
if (tMin > tMax)
return false;
}
else{
if (-e + aabb_min.z > 0.0f || -e + aabb_max.z < 0.0f)
return false;
}
}
intersection_distance = tMin;
return true;
}
RagDoll::RagDoll (btDynamicsWorld* ownerWorld, const btVector3& positionOffset,
btScalar scale_ragdoll) : m_ownerWorld (ownerWorld) {
// setup colors
for (int i = 0; i < BODYPART_COUNT; i++) {
// ofVec4f col = ofVec4f(ofRandom(0.7, 1.0),
// ofRandom(0.5, 1.0),
// ofRandom(0.7, 1.0),
// 1.0);
ofVec4f col = ofVec4f(0.7,
0.7,
0.7,
1.0);
colors.push_back(col);
}
// Setup the geometry
m_shapes[BODYPART_PELVIS] = new btCapsuleShape(btScalar(scale_ragdoll*0.15), btScalar(scale_ragdoll*0.20));
m_shapes[BODYPART_SPINE] = new btCapsuleShape(btScalar(scale_ragdoll*0.15), btScalar(scale_ragdoll*0.28));
m_shapes[BODYPART_HEAD] = new btCapsuleShape(btScalar(scale_ragdoll*0.10), btScalar(scale_ragdoll*0.05));
m_shapes[BODYPART_LEFT_UPPER_LEG] = new btCapsuleShape(btScalar(scale_ragdoll*0.07), btScalar(scale_ragdoll*0.45));
m_shapes[BODYPART_LEFT_LOWER_LEG] = new btCapsuleShape(btScalar(scale_ragdoll*0.05), btScalar(scale_ragdoll*0.37));
m_shapes[BODYPART_RIGHT_UPPER_LEG] = new btCapsuleShape(btScalar(scale_ragdoll*0.07), btScalar(scale_ragdoll*0.45));
m_shapes[BODYPART_RIGHT_LOWER_LEG] = new btCapsuleShape(btScalar(scale_ragdoll*0.05), btScalar(scale_ragdoll*0.37));
m_shapes[BODYPART_LEFT_UPPER_ARM] = new btCapsuleShape(btScalar(scale_ragdoll*0.05), btScalar(scale_ragdoll*0.33));
m_shapes[BODYPART_LEFT_LOWER_ARM] = new btCapsuleShape(btScalar(scale_ragdoll*0.04), btScalar(scale_ragdoll*0.25));
m_shapes[BODYPART_RIGHT_UPPER_ARM] = new btCapsuleShape(btScalar(scale_ragdoll*0.05), btScalar(scale_ragdoll*0.33));
m_shapes[BODYPART_RIGHT_LOWER_ARM] = new btCapsuleShape(btScalar(scale_ragdoll*0.04), btScalar(scale_ragdoll*0.25));
// Setup all the rigid bodies
btTransform offset; offset.setIdentity();
offset.setOrigin(positionOffset);
// fix the rotation
double angle = (double)180*(TWO_PI/360.0);
btVector3 zaxis(0.0,0.0,1.0);
btQuaternion zquat(zaxis,angle);
btVector3 yaxis(0.0,1.0,0.0);
btQuaternion yquat(yaxis,angle);
btQuaternion quat = zquat*yquat;
offset.setRotation(quat);
btTransform transform;
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.), btScalar(scale_ragdoll*1.), btScalar(0.)));
m_bodies[BODYPART_PELVIS] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_PELVIS]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.), btScalar(scale_ragdoll*1.2), btScalar(0.)));
m_bodies[BODYPART_SPINE] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_SPINE]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.), btScalar(scale_ragdoll*1.6), btScalar(0.)));
m_bodies[BODYPART_HEAD] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_HEAD]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.18*scale_ragdoll), btScalar(0.65*scale_ragdoll),
btScalar(0.)));
m_bodies[BODYPART_LEFT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.18*scale_ragdoll), btScalar(0.2*scale_ragdoll), btScalar(0.)));
m_bodies[BODYPART_LEFT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.18*scale_ragdoll), btScalar(0.65*scale_ragdoll), btScalar(0.)));
m_bodies[BODYPART_RIGHT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.18*scale_ragdoll), btScalar(0.2*scale_ragdoll), btScalar(0.)));
m_bodies[BODYPART_RIGHT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.35*scale_ragdoll), btScalar(1.45*scale_ragdoll), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,SIMD_HALF_PI);
m_bodies[BODYPART_LEFT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_ARM]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.7*scale_ragdoll), btScalar(1.45*scale_ragdoll), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,SIMD_HALF_PI);
m_bodies[BODYPART_LEFT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_ARM]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.35*scale_ragdoll), btScalar(1.45*scale_ragdoll), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,-SIMD_HALF_PI);
m_bodies[BODYPART_RIGHT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_ARM]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.7*scale_ragdoll), btScalar(1.45*scale_ragdoll), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,-SIMD_HALF_PI);
m_bodies[BODYPART_RIGHT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_ARM]);
// Setup some damping on the m_bodies
for (int i = 0; i < BODYPART_COUNT; ++i)
{
m_bodies[i]->setDamping(0.05, 0.85);
m_bodies[i]->setDeactivationTime(0.8);
m_bodies[i]->setSleepingThresholds(1.6, 2.5);
}
///////////////////////////// SETTING THE CONSTRAINTS /////////////////////////////////////////////7777
// Now setup the constraints
btGeneric6DofConstraint * joint6DOF;
btTransform localA, localB;
bool useLinearReferenceFrameA = true;
/// ******* SPINE HEAD ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.), btScalar(0.30*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_PI*0.3f,-SIMD_EPSILON,-SIMD_PI*0.3f));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.5f,SIMD_EPSILON,SIMD_PI*0.3f));
#endif
m_joints[JOINT_SPINE_HEAD] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true);
}
/// *************************** ///
/// ******* LEFT SHOULDER ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(-0.2*scale_ragdoll), btScalar(0.15*scale_ragdoll), btScalar(0.)));
localB.getBasis().setEulerZYX(SIMD_HALF_PI,0,-SIMD_HALF_PI);
localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_PI*0.8f,-SIMD_EPSILON,-SIMD_PI*0.5f));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.8f,SIMD_EPSILON,SIMD_PI*0.5f));
#endif
m_joints[JOINT_LEFT_SHOULDER] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true);
}
/// *************************** ///
/// ******* RIGHT SHOULDER ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.2*scale_ragdoll), btScalar(0.15*scale_ragdoll), btScalar(0.)));
localB.getBasis().setEulerZYX(0,0,SIMD_HALF_PI);
localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_PI*0.8f,-SIMD_EPSILON,-SIMD_PI*0.5f));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.8f,SIMD_EPSILON,SIMD_PI*0.5f));
#endif
m_joints[JOINT_RIGHT_SHOULDER] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true);
}
/// *************************** ///
/// ******* LEFT ELBOW ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint (*m_bodies[BODYPART_LEFT_UPPER_ARM], *m_bodies[BODYPART_LEFT_LOWER_ARM], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.7,SIMD_EPSILON,SIMD_EPSILON));
#endif
m_joints[JOINT_LEFT_ELBOW] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true);
}
/// *************************** ///
/// ******* RIGHT ELBOW ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint (*m_bodies[BODYPART_RIGHT_UPPER_ARM], *m_bodies[BODYPART_RIGHT_LOWER_ARM], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.7,SIMD_EPSILON,SIMD_EPSILON));
#endif
m_joints[JOINT_RIGHT_ELBOW] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true);
}
/// *************************** ///
/// ******* PELVIS ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,SIMD_HALF_PI,0);
localA.setOrigin(btVector3(btScalar(0.), btScalar(0.15*scale_ragdoll), btScalar(0.)));
localB.getBasis().setEulerZYX(0,SIMD_HALF_PI,0);
localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.15*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint (*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_PI*0.2,-SIMD_EPSILON,-SIMD_PI*0.3));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.2,SIMD_EPSILON,SIMD_PI*0.6));
#endif
m_joints[JOINT_PELVIS_SPINE] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true);
}
/// *************************** ///
/// ******* LEFT HIP ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(-0.18*scale_ragdoll), btScalar(-0.10*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_HALF_PI*0.5,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_HALF_PI*0.8,SIMD_EPSILON,SIMD_HALF_PI*0.6f));
#endif
m_joints[JOINT_LEFT_HIP] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true);
}
/// *************************** ///
/// ******* RIGHT HIP ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.18*scale_ragdoll), btScalar(-0.10*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_HALF_PI*0.5,-SIMD_EPSILON,-SIMD_HALF_PI*0.6f));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_HALF_PI*0.8,SIMD_EPSILON,SIMD_EPSILON));
#endif
m_joints[JOINT_RIGHT_HIP] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true);
}
/// *************************** ///
/// ******* LEFT KNEE ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint (*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB,useLinearReferenceFrameA);
//
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.7f,SIMD_EPSILON,SIMD_EPSILON));
#endif
m_joints[JOINT_LEFT_KNEE] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true);
}
/// *************************** ///
/// ******* RIGHT KNEE ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint (*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.7f,SIMD_EPSILON,SIMD_EPSILON));
#endif
m_joints[JOINT_RIGHT_KNEE] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true);
}
/// *************************** ///
}
bool MPUtil::RayOBBIntersection(
DCCore::Point_3 vA, // Ray origin, in world space
DCCore::Point_3 vB, // Ray direction (NOT target position!), in world space. Must be normalize()'d.
DCCore::Point_3 aabb_min, // Minimum X,Y,Z coords of the mesh when not transformed at all.
DCCore::Point_3 aabb_max, // Maximum X,Y,Z coords. Often aabb_min*-1 if your mesh is centered, but it's not always the case.
const double* ModelMatrix, // Transformation applied to the mesh (which will thus be also applied to its bounding box)
float& intersection_distance // Output : distance between ray_origin and the intersection with the OBB
)
{
DCCore::Point_3 ray_origin = vA; // Ray origin, in world space
DCCore::Point_3 ray_direction ;
DCCore::Point_3 temp = (vB-vA);
temp.Normalize();
ray_direction = temp;
// Intersection method from Real-Time Rendering and Essential Mathematics for Games
float tMin = 0.0f;
float tMax = 100000.0f;
DCCore::Point_3 OBBposition_worldspace(0, 0, 0);
DCCore::Point_3 delta = OBBposition_worldspace - ray_origin;
// Test intersection with the 2 planes perpendicular to the OBB's X axis
{
DCCore::Point_3 xaxis(1, 0, 0);
float e = xaxis.Dot(delta);
float f = ray_direction.Dot(xaxis);
if ( fabs(f) > 0.001f ){ // Standard case
float t1 = (e+aabb_min.x)/f; // Intersection with the "left" plane
float t2 = (e+aabb_max.x)/f; // Intersection with the "right" plane
// t1 and t2 now contain distances betwen ray origin and ray-plane intersections
// We want t1 to represent the nearest intersection,
// so if it's not the case, invert t1 and t2
if (t1>t2){
float w=t1;t1=t2;t2=w; // swap t1 and t2
}
// tMax is the nearest "far" intersection (amongst the X,Y and Z planes pairs)
if ( t2 < tMax )
tMax = t2;
// tMin is the farthest "near" intersection (amongst the X,Y and Z planes pairs)
if ( t1 > tMin )
tMin = t1;
// And here's the trick :
// If "far" is closer than "near", then there is NO intersection.
// See the images in the tutorials for the visual explanation.
if (tMax < tMin )
return false;
}else{ // Rare case : the ray is almost parallel to the planes, so they don't have any "intersection"
if(-e+aabb_min.x > 0.0f || -e+aabb_max.x < 0.0f)
return false;
}
}
// Test intersection with the 2 planes perpendicular to the OBB's Y axis
// Exactly the same thing than above.
{
DCCore::Point_3 yaxis(0, 1, 0);
float e = yaxis.Dot(delta);
float f = ray_direction.Dot(yaxis);
if ( fabs(f) > 0.001f ){
float t1 = (e+aabb_min.y)/f;
float t2 = (e+aabb_max.y)/f;
if (t1>t2){float w=t1;t1=t2;t2=w;}
if ( t2 < tMax )
tMax = t2;
if ( t1 > tMin )
tMin = t1;
if (tMin > tMax)
return false;
}else{
if(-e+aabb_min.y > 0.0f || -e+aabb_max.y < 0.0f)
return false;
}
}
// Test intersection with the 2 planes perpendicular to the OBB's Z axis
// Exactly the same thing than above.
{
DCCore::Point_3 zaxis(0, 0, 1);
float e = zaxis.Dot(delta);
float f = ray_direction.Dot(zaxis);
if ( fabs(f) > 0.001f ){
float t1 = (e+aabb_min.z)/f;
float t2 = (e+aabb_max.z)/f;
if (t1>t2){float w=t1;t1=t2;t2=w;}
if ( t2 < tMax )
tMax = t2;
if ( t1 > tMin )
tMin = t1;
if (tMin > tMax)
return false;
}else{
if(-e+aabb_min.z > 0.0f || -e+aabb_max.z < 0.0f)
return false;
}
}
intersection_distance = tMin;
return true;
}
/*! The mouseMove is called by the viewer when the mouse is moved in the
viewer and this handle is the active one.
\param x the x-pos of the mouse (pixel)
\param y the y-pos of the mouse (pixel)
*/
void PlaneMoveManipulator::mouseMove(const Int16 x,
const Int16 y)
{
SLOG << "==============================" << endLog;
SLOG << "PlaneMoveManipulator::mouseMove() enter x=" << x << " y=" << y << endLog;
// get the beacon's core (must be ComponentTransform) and it's center
if( getTarget() == NULL )
{
SWARNING << "Handle has no target.\n";
return;
}
// get transformation of beacon
Transform *t = dynamic_cast<Transform *>(getTarget()->getCore());
if( t == NULL )
{
SWARNING << "handled object has no parent transform!\n";
return;
}
Vec3f translation; // for matrix decomposition
Quaternion rotation;
Vec3f scaleFactor;
Quaternion scaleOrientation;
t->getMatrix().getTransform(translation, rotation, scaleFactor,
scaleOrientation);
OSG::Line viewray;
getViewport()->getCamera()->calcViewRay(viewray, x, y, *getViewport());
SLOG << "Manipulator::mouseMove(): viewray: " << viewray << endLog;
// Get manipulator axes into world space
OSG::Matrix tm = getTarget()->getToWorld();
Vec3f rot_axis;
tm.multFull(Vec3f(0,1,0), rot_axis);
Plane pl(rot_axis, getClickPoint());
Pnt3f plpoint;
if (pl.intersect(viewray, plpoint) == true) // Ignore moving out of the plane...
{
SLOG << "Manipulator::mouseMove(): plpoint: " << plpoint << endLog;
Vec3f trans = getBaseTranslation();
Quaternion rot = getBaseRotation();
// Get manipulator axes into world space
Vec3f xp,zp;
tm.multFull(Vec3f(1,0,0), xp);
tm.multFull(Vec3f(0,0,1), zp);
if (getActiveSubHandle() == getHandleXNode())
{
Line xaxis(getClickPoint(), xp);
Line zaxis(getClickPoint(), zp);
Real32 fx = xaxis.getClosestPointT(plpoint);
Real32 fz = zaxis.getClosestPointT(plpoint);
SLOG << "Manipulator::mouseMove(): xaxis: " << xaxis << " zaxis: " << zaxis <<endLog;
SLOG << "Manipulator::mouseMove(): fx: " << fx << " fz: " << fz <<endLog;
// Alternative: transform hitpoint into manip space
OSG::Matrix m = getTarget()->getToWorld();
m.invert();
Pnt3f mpoint;
m.mult(plpoint, mpoint);
SLOG << "Manipulator::mouseMove(): mpoint:" << mpoint << endLog;
trans = trans + xp * fx + zp * fz;
}
else if (getActiveSubHandle() == getHandleZNode())
{
Pnt3f wcenter;
tm.multFull(Pnt3f(0,getLength()[1],0), wcenter);
Vec3f vclick, vcurrent;
vclick = getClickPoint() - wcenter;
vcurrent = plpoint - wcenter;
vclick.normalize();
vcurrent.normalize();
Real32 a = vclick.enclosedAngle(vcurrent);
SLOG << "Manipulator::mouseMove(): wcenter:" << wcenter << "" <<endLog;
SLOG << "Manipulator::mouseMove(): vclick:" << vclick << " vcurrent:" << vcurrent <<endLog;
SLOG << "Manipulator::mouseMove(): angle:" << a << " deg: " << osgRad2Degree(a) << endLog;
}
Matrix m;
m.setTransform(trans, rot, scaleFactor, scaleOrientation);
t->setMatrix(m);
}
setLastMousePos(Pnt2f(Real32(x), Real32(y)));
updateHandleTransform();
//SLOG << "Manipulator::mouseMove() leave\n" << std::flush;
}
bool Matrix::decompose(Vector3* scale, Quaternion* rotation, Vector3* translation) const
{
if (translation)
{
// Extract the translation.
translation->x = m[12];
translation->y = m[13];
translation->z = m[14];
}
// Nothing left to do.
if (scale == NULL && rotation == NULL)
return true;
// Extract the scale.
// This is simply the length of each axis (row/column) in the matrix.
Vector3 xaxis(m[0], m[1], m[2]);
float scaleX = xaxis.length();
Vector3 yaxis(m[4], m[5], m[6]);
float scaleY = yaxis.length();
Vector3 zaxis(m[8], m[9], m[10]);
float scaleZ = zaxis.length();
// Determine if we have a negative scale (true if determinant is less than zero).
// In this case, we simply negate a single axis of the scale.
float det = determinant();
if (det < 0)
scaleZ = -scaleZ;
if (scale)
{
scale->x = scaleX;
scale->y = scaleY;
scale->z = scaleZ;
}
// Nothing left to do.
if (rotation == NULL)
return true;
// Scale too close to zero, can't decompose rotation.
if (scaleX < MATH_TOLERANCE || scaleY < MATH_TOLERANCE || fabs(scaleZ) < MATH_TOLERANCE)
return false;
float rn;
// Factor the scale out of the matrix axes.
rn = 1.0f / scaleX;
xaxis.x *= rn;
xaxis.y *= rn;
xaxis.z *= rn;
rn = 1.0f / scaleY;
yaxis.x *= rn;
yaxis.y *= rn;
yaxis.z *= rn;
rn = 1.0f / scaleZ;
zaxis.x *= rn;
zaxis.y *= rn;
zaxis.z *= rn;
// Now calculate the rotation from the resulting matrix (axes).
float trace = xaxis.x + yaxis.y + zaxis.z + 1.0f;
if (trace > MATH_EPSILON)
{
float s = 0.5f / sqrt(trace);
rotation->w = 0.25f / s;
rotation->x = (yaxis.z - zaxis.y) * s;
rotation->y = (zaxis.x - xaxis.z) * s;
rotation->z = (xaxis.y - yaxis.x) * s;
}
else
{
// Note: since xaxis, yaxis, and zaxis are normalized,
// we will never divide by zero in the code below.
if (xaxis.x > yaxis.y && xaxis.x > zaxis.z)
{
float s = 0.5f / sqrt(1.0f + xaxis.x - yaxis.y - zaxis.z);
rotation->w = (yaxis.z - zaxis.y) * s;
rotation->x = 0.25f / s;
rotation->y = (yaxis.x + xaxis.y) * s;
rotation->z = (zaxis.x + xaxis.z) * s;
}
else if (yaxis.y > zaxis.z)
{
float s = 0.5f / sqrt(1.0f + yaxis.y - xaxis.x - zaxis.z);
rotation->w = (zaxis.x - xaxis.z) * s;
rotation->x = (yaxis.x + xaxis.y) * s;
rotation->y = 0.25f / s;
rotation->z = (zaxis.y + yaxis.z) * s;
}
else
{
float s = 0.5f / sqrt(1.0f + zaxis.z - xaxis.x - yaxis.y );
rotation->w = (xaxis.y - yaxis.x ) * s;
rotation->x = (zaxis.x + xaxis.z ) * s;
rotation->y = (zaxis.y + yaxis.z ) * s;
rotation->z = 0.25f / s;
}
}
return true;
}
BZ_END_STENCIL
/*
* Allocate arrays and set their initial state
*/
void setup(const int N, vectorField& V, vectorField& nextV, scalarField& P,
scalarField& P_rhs, vectorField& advect, vectorField& force)
{
// A 1m x 1m x 1m domain
spatialStep = 1.0 / (N - 1);
geom = UniformCubicGeometry<3>(spatialStep);
// Allocate arrays
allocateArrays(shape(N,N,N), advect, V, nextV, force); // vector fields
allocateArrays(shape(N,N,N), P, P_rhs); // scalar fields
// Since incompressibility is assumed, pressure only shows up as
// derivative terms in the equations. We choose airPressure = 0
// as an arbitrary datum.
airPressure = 0; // Pa
rho = 1000; // density of fluid, kg/m^3
recip_rho = 1.0 / rho; // inverse of density
eta = 1.0e-6; // kinematic viscosity of fluid, m^2/s
gravity = 9.81; // m/s^2
delta_t = 0.001; // initial time step, in seconds
volume = pow3(spatialStep); // cubic volume associated with grid point
// Kludge: Set eta high, so that the flow will spread faster.
// This means the cube is filled with molasses, rather than water.
eta *= 1000;
// Initial conditions: quiescent
V = 0.0;
P_rhs = 0.0;
advect = 0.0;
nextV = 0.0;
// Initial pressure condition: gravity causes a linear increase
// in pressure with depth. Note that tensor::k means the index
// associated with the z axis (they are labelled i, j, k).
#ifdef NO_GRAVITY
gravityPressureGradient = 0.0;
P = 0.0;
#else
gravityPressureGradient = spatialStep * gravity * rho;
#ifdef BZ_HAVE_NAMESPACES
P = airPressure + tensor::k * gravityPressureGradient;
#else
P = airPressure + k * gravityPressureGradient;
#endif
#endif
// Set up the forcing function: gravity plus a stirring force
// at the bottom
double gravity_z = gravity * rho;
const int x = 0, y = 1, z = 2;
force[x] = 0.0;
force[y] = 0.0;
#ifdef NO_GRAVITY
force[z] = 0.0;
#else
force[z] = gravity_z;
#endif
#ifndef NO_STIRRING
// Centre of the stirring
int centrex = int(2 * N / 3.0);
int centrey = int(2 * N / 3.0);
int centrez = int(4 * N / 5.0);
const double stirRadius = 1.0 / 3.0;
vector3d zaxis(0,0,1);
// Loop through the 2D slice where the stirring occurs
for (int i=force.lbound(firstDim); i <= force.ubound(firstDim); ++i)
{
for (int j=force.lbound(secondDim); j <= force.ubound(secondDim); ++j)
{
// Vector from the centre of the stirring to the current
// coordinate
vector3d r((i-centrex) * spatialStep, (j-centrey) * spatialStep, 0.0);
if (norm(r) < stirRadius)
{
// The cross product of the z-axis and the vector3d to this
// coordinate yields the direction of the force. Multiply
// by gravity to get a reasonable magnitude (max force =
// 5 * gravity)
force(i,j,centrez) += cross(zaxis, r)
* (5 * gravity_z / stirRadius);
}
}
}
#endif // NO_STIRRING
}
