void virtuose::fillSpeed(VRPhysics* p, float* to, VRPhysics* origin) {
Vec3d vel = p->getLinearVelocity();
if (origin!=0) vel -= origin->getLinearVelocity();
to[0] = vel.z();
to[1] = vel.x();
to[2] = vel.y();
Vec3d ang = p->getAngularVelocity();
if (origin!=0) ang -= origin->getAngularVelocity();
to[3] = ang.z();
to[4] = ang.x();
to[5] = ang.y();
}
std::vector<double> Cuboid::getCoordinate( Point v )
{
std::vector<double> coords(3);
// Map points to uniform box coordinates
QSurfaceMesh currBoxMesh = getGeometry();
QSurfaceMesh currUniformBoxMesh = getBoxGeometry(currBox, true);
v = MeanValueCooridnates::point(
MeanValueCooridnates::weights(v, &currBoxMesh), &currUniformBoxMesh);
Vec3d pos = getCoordinatesInUniformBox(currBox, v);
coords[0] = pos.x();
coords[1] = pos.y();
coords[2] = pos.z();
return coords;
}
void Camera::init(){
//Matrix4d::zero(transformMatrix);
// initalize view center
Vec3d orient = (focus-position);
orient.normalize();
viewCenter = position + orient * distance;
// initialzie horiVec and vertVec
if ((fabs(orient.y) > Epsilon) || (fabs(orient.x) > Epsilon)){
horiVec = Vec3d(-orient.y, orient.x, 0);
}
else{
horiVec = Vec3d(-orient.z, 0, orient.x);
}
vertVec = Vec3d::cross(horiVec, orient);
vertVec.normalize();
horiVec.normalize();
vertVec *= distance * tan(vAngle/2);
horiVec *= distance * tan(hAngle/2);
}
Group Transform::apply(const Group& g)
{
std::vector<Vec3d> vv;
Vec3d vtmp;
double x1, y1, z1;
int s = g.n_atom_;
for (int i = 0; i < s; ++i) {
vtmp = g.coordinates_[i];
x1 = rotv1_ * vtmp;
y1 = rotv2_ * vtmp;
z1 = rotv3_ * vtmp;
vtmp.set_coordinate(x1, y1, z1);
vtmp += translation_;
vv.push_back(vtmp);
}
Group gtmp(vv);
return gtmp;
}
//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
TEST(VariantTest, TypeVec3d)
{
const Vec3d val(1.0, -2000.1234, 3000.6789);
Variant var(val);
ASSERT_EQ(Variant::VEC3D, var.type());
ASSERT_TRUE(var.isValid());
Vec3d v = var.getVec3d();
ASSERT_EQ(val.x(), v.x());
ASSERT_EQ(val.y(), v.y());
ASSERT_EQ(val.z(), v.z());
}
Vec3d rectToSpherical(const Vec3d& v)
{
double r = v.length();
double theta = atan2(v.y, v.x);
if (theta < 0)
theta = theta + 2 * PI;
double phi = asin(v.z / r);
return Vec3d(theta, phi, r);
}
void add_point(const Vec3d& point){
uchar b = ~bits_; //Flip bits
b &= -b; //Last set (available) bit
uchar pos = s_pos[b]; //Get the bit position from the lookup table
last_sb_ = pos;
bits_ |= b; //Insert the new bit
++size_;
p_[pos] = point;
double l2 = point.length2();
if(l2 > max_vert2) max_vert2 = l2;
}
/*
* Check existance of obstacles. The route is specified by
* source and destination. If targetBurdenLoc is given, it is
* excluded from obstacles. If targetBurdenLoc==null, ignored.
*/
bool SampleBase::checkAllConflict(const Vec3d& src,const Vec3d& dest,const Vec3d& targetBurdenLoc) {
vector<Vec3d> array = burdenSet.getVectors();
for (vector<Vec3d>::iterator itr = array.begin(); itr != array.end(); itr++) {
Vec3d v = (*itr);
if (v == src)
continue;
if (v == dest)
continue;
if (targetBurdenLoc.x<1000 && v.epsilonEquals(targetBurdenLoc,1.0))
continue;
if (checkConflict(src,dest,v,1.5)) {
return true;
}
}
if (checkConflict(src,dest,obstacle1,3.0))
return true;
if (checkConflict(src,dest,obstacle2,3.0))
return true;
return false;
}
Transformation3d :: Transformation3d (const Vec3d & translate)
{
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
lin[i][j] = 0;
for (int i = 0; i < 3; i++)
{
offset[i] = translate.X(i+1);
lin[i][i] = 1;
}
}
void SofaHAPIHapticsDevice::draw(const sofa::core::visual::VisualParams* vparams)
{
if (!device.get()) return;
if(drawDevice.getValue())
{
// TODO
HAPI::HAPIHapticsDevice::DeviceValues dv = device->getDeviceValues();
/// COMPUTATION OF THE virtualTool 6D POSITION IN THE World COORDINATES
Vec3d pos = conv(dv.position);
Vec3d force = conv(dv.force);
Quat quat = conv(dv.orientation);
quat.normalize();
Transform baseDevice_H_endDevice(pos*data.scale, quat);
Transform world_H_virtualTool = data.world_H_baseDevice * baseDevice_H_endDevice * data.endDevice_H_virtualTool;
Vec3d wpos = world_H_virtualTool.getOrigin();
vparams->drawTool()->setLightingEnabled(true); //Enable lightning
if (drawHandleSize.getValue() == 0.0f)
{
std::vector<Vec3d> points;
points.push_back(wpos);
vparams->drawTool()->drawSpheres(points, 1.0f, sofa::defaulttype::Vec<4,float>(0,0,1,1));
}
else
{
Vec3d wposH = wpos + data.world_H_baseDevice.projectVector( baseDevice_H_endDevice.projectVector(Vec3d(0.0,0.0,drawHandleSize.getValue())));
vparams->drawTool()->drawArrow(wposH, wpos, drawHandleSize.getValue()*0.05f, sofa::defaulttype::Vec<4,float>(0,0,1,1));
}
if (force.norm() > 0 && drawForceScale.getValue() != 0.0f)
{
//std::cout << "F = " << force << std::endl;
Vec3d fscaled = force*(drawForceScale.getValue()*data.scale);
Transform baseDevice_H_endDeviceF(pos*data.scale+fscaled, quat);
Transform world_H_virtualToolF = data.world_H_baseDevice * baseDevice_H_endDeviceF * data.endDevice_H_virtualTool;
Vec3d wposF = world_H_virtualToolF.getOrigin();
vparams->drawTool()->drawArrow(wpos, wposF, 0.1f, sofa::defaulttype::Vec<4,float>(1,0,0,1));
}
vparams->drawTool()->setLightingEnabled(false); //Disable lightning
}
}
void ParticleSystem::constraintSpring( double dt )
{
for (int iter = 0; iter < 20; iter++)
{
for (size_t i = 0; i < springs.size(); i++)
{
if (springs[i].type == Spring::SPRING_BEND)
{
continue;
}
int p1 = springs[i].p1;
int p2 = springs[i].p2;
double l0 = springs[i].l0;
Vec3d dx = pts[p1].cp - pts[p2].cp;
Vec3d c = (pts[p1].cp + pts[p2].cp) / 2;
double d = sqrt(dx.ddot(dx));
if (d > maxSpringLen * l0)
{
pts[p1].cp = c + dx * maxSpringLen * l0 / (d * 2);
pts[p2].cp = c - dx * maxSpringLen * l0 / (d * 2);
}
if (d < minSpringLen * l0)
{
pts[p1].cp = c + dx * minSpringLen * l0 / (d * 2);
pts[p2].cp = c - dx * minSpringLen * l0 / (d * 2);
}
}
}
// recalculate the velocity
for (size_t i = 0; i < pts.size(); i++)
{
if (pts[i].isPinned)
{
pts[i].cp = pts[i].op;
}
pts[i].v = (pts[i].cp - pts[i].op) / dt;
}
}
namespace BlockWorld {
static Vec3d scaling;
static Vec3d pos0;
static Mat3d rotations[6];
void drawBlock( Uint16 shape, Uint8 orientation, Uint8 ix, Uint8 iy, Uint8 iz ){
Mat3d rotmat = rotations[ orientation & 0b00011111 ];
rotmat.a.mul( scaling.x );
rotmat.b.mul( scaling.y );
rotmat.c.mul( scaling.z );
if (orientation & 0b10000000) rotmat.a.mul( -1.0d );
if (orientation & 0b01000000) rotmat.b.mul( -1.0d );
if (orientation & 0b00100000) rotmat.c.mul( -1.0d );
Vec3d pos; pos.set( ix*scaling.x+pos0.x, iy*scaling.y+pos0.y, iz*scaling.z+pos0.z );
float glMat[16];
glPushMatrix ( );
/*
printf( " pos (%3.3f,%3.3f,%3.3f) \n", pos.x, pos.y, pos.z );
printf( " a (%3.3f,%3.3f,%3.3f) \n", rotmat.a.x, rotmat.a.x, rotmat.a.x );
printf( " b (%3.3f,%3.3f,%3.3f) \n", rotmat.b.x, rotmat.b.x, rotmat.b.x );
printf( " c (%3.3f,%3.3f,%3.3f) \n", rotmat.c.x, rotmat.c.x, rotmat.c.x );
*/
Draw3D::toGLMat( pos, rotmat, glMat );
glMultMatrixf( glMat );
glCallList ( shape );
glPopMatrix ( );
}
void buildRotations( const Mat3d& rot ){
rotations[0].set( rot.a, rot.b, rot.c );
rotations[1].set( rot.a, rot.c, rot.b );
rotations[2].set( rot.b, rot.a, rot.c );
rotations[3].set( rot.b, rot.c, rot.a );
rotations[4].set( rot.c, rot.a, rot.b );
rotations[5].set( rot.c, rot.b, rot.a );
}
void setupBlockWorld( const Vec3d& pos0_, const Vec3d& scaling_, const Mat3d& rot ){
pos0 .set( pos0_ );
scaling.set( scaling_ );
buildRotations( rot );
/*
printf( " pos (%3.3f,%3.3f,%3.3f) \n", pos0.x, pos0.y, pos0.z );
//printf( " scaling_ (%3.3f,%3.3f,%3.3f) \n", scaling_.x, scaling_.y, scaling_.z );
printf( " scaling (%3.3f,%3.3f,%3.3f) \n", scaling.x, scaling.y, scaling.z );
printf( " a (%3.3f,%3.3f,%3.3f) \n", rotations[0].a.x, rotations[0].a.y, rotations[0].a.z );
printf( " b (%3.3f,%3.3f,%3.3f) \n", rotations[0].b.x, rotations[0].b.y, rotations[0].b.z );
printf( " c (%3.3f,%3.3f,%3.3f) \n", rotations[0].c.x, rotations[0].c.y, rotations[0].c.z );
*/
}
};
bool Geometry::intersect(const ray&r, isect&i) const
{
// Transform the ray into the object's local coordinate space
Vec3d pos = transform->globalToLocalCoords(r.getPosition());
Vec3d dir = transform->globalToLocalCoords(r.getPosition() + r.getDirection()) - pos;
double length = dir.length();
dir /= length;
ray localRay( pos, dir, r.type() );
if (intersectLocal(localRay, i)) {
// Transform the intersection point & normal returned back into global space.
i.N = transform->localToGlobalCoordsNormal(i.N);
i.t /= length;
return true;
} else {
return false;
}
}
int ClosedPolygon::insertIndexedPoint(const Vec3d &p)
{
int vIndex = -1;
kdres * findP = points.nearest3f(p.x(), p.y(), p.z());
if(findP)
{
double * pos = findP->riter->item->pos;
Vec3d closestPoint(pos[0], pos[1], pos[2]);
double dist = (closestPoint - p).norm();
if(dist < closedPolyEpsilon)
{
vIndex = findP->riter->item->index;
kd_res_free(findP);
return vIndex;
}
}
vIndex = lastVertexIndex;
allPoints[vIndex] = p;
points.insert3f(p.x(), p.y(), p.z(), lastVertexIndex++);
return vIndex;
}
bool Triangle::angleCriterion( const double &minCosAngle,
const double &maxCosAngle )
{
Vec3d ab = m_vertices[1].m_xyz - m_vertices[0].m_xyz;
Vec3d bc = m_vertices[2].m_xyz - m_vertices[1].m_xyz;
Vec3d ac = m_vertices[2].m_xyz - m_vertices[0].m_xyz;
double lenAB = ab.dot(ab);
double lenBC = bc.dot(bc);
double lenAC = ac.dot(ac);
double maxLen = lenAB, minLen = lenAB;
double lenMaxE0 = lenBC, lenMaxE1 = lenAC;
double lenMinE0 = lenBC, lenMinE1 = lenAC;
Vec3d maxE0 = bc, maxE1 = ac, minE0 = bc, minE1 = ac;
bool maxFlag = true, minFlag = true;
if (maxCosAngle > COS180 && maxCosAngle < COS60)
{
if (maxLen < lenBC)
{
maxLen = lenBC;
lenMaxE0 = lenAB;
lenMaxE1 = lenAC;
maxE0 = ab;
maxE1 = ac;
}
if (maxLen < lenAC)
{
lenMaxE0 = lenAB;
lenMaxE1 = lenBC;
maxE0 = -ab;
maxE1 = bc;
}
maxFlag = maxE0.dot(maxE1) > sqrt(lenMaxE0)
* sqrt(lenMaxE1) * maxCosAngle;
}
if (minCosAngle < COS0 && minCosAngle > COS60)
{
if (minLen > lenBC)
{
minLen = lenBC;
lenMinE0 = lenAB;
lenMinE1 = lenAC;
minE0 = ab;
minE1 = ac;
}
if (minLen > lenAC)
{
lenMinE0 = lenAB;
lenMinE1 = lenBC;
minE0 = -ab;
minE1 = bc;
}
minFlag = minE0.dot(minE1) < sqrt(lenMinE0)
* sqrt(lenMinE1) * minCosAngle;
}
return minFlag && maxFlag;
}
LineFieldRenderer::LineFieldRenderer(const Manifold& m, bool smooth, VertexAttributeVector<Vec3d>& lines, float _r):
SimpleShaderRenderer(vss,fss), r(_r)
{
float noise_scale = 10.0f/r;
float line_scale = 0.003f;
GLint old_prog;
glGetIntegerv(GL_CURRENT_PROGRAM, &old_prog);
glUseProgram(prog);
glUniform1fARB(glGetUniformLocationARB(prog, "scale_line"),line_scale*noise_scale);
glUniform1fARB(glGetUniformLocationARB(prog, "noise_scale"),noise_scale);
glUniform1iARB(glGetUniformLocationARB(prog, "noise_tex"),0);
GLuint direction = glGetAttribLocation(prog, "direction");
glNewList(display_list,GL_COMPILE);
for(FaceIDIterator f = m.faces_begin(); f != m.faces_end(); ++f) {
if(!smooth)
glNormal3dv(normal(m, *f).get());
if(no_edges(m, *f) == 3)
glBegin(GL_TRIANGLES);
else
glBegin(GL_POLYGON);
for(Walker w = m.walker(*f); !w.full_circle(); w = w.circulate_face_ccw()) {
Vec3d n(normal(m, w.vertex()));
if(smooth)
glNormal3dv(n.get());
Vec3d d = lines[w.vertex()];
d = normalize(d-n*dot(n,d));
glVertexAttrib3dv(direction, d.get());
glVertex3dv(m.pos(w.vertex()).get());
}
glEnd();
}
glBindTexture(GL_TEXTURE_3D, 0);
glEndList();
glUseProgram(old_prog);
}
void Mesh::inner( int i )
{
Vertex v = m_vertices[i];
if (v.m_neighbors[0] == 0)
{
m_vertices[i].m_isInner = true;
return;
}
int pcnt = 0, ncnt = 0;
for (int j = 1; j < v.m_neighbors[0]; j++)
{
if (m_vertices[v.m_neighbors[j]].m_neighbors[0] == 0)
{
continue;
}
Vec3d vec = m_vertices[v.m_neighbors[j]].m_xyz - v.m_xyz;
double val = vec.ddot(v.m_normal);
if (val > 0)
{
pcnt++;
}
else
{
ncnt++;
}
}
if (ncnt < pcnt)
{
m_vertices[i].m_normal = -m_vertices[i].m_normal;
int tmp = ncnt;
ncnt = pcnt;
pcnt = tmp;
}
if (double(pcnt) / double(pcnt + ncnt) > INNER_THRESHOLD)
{
m_vertices[i].m_isInner = true;
return;
}
}
void GraphicCamera::MouseEventHandler(int button, int state, int x, int y){
double realy, wx, wy, wz;
// if ALT key used
// int mode = glutGetModifiers();
if (state == GLUT_UP && CameraMode != kINACTIVE) {
current_elev += dt_elev;
current_azim += dt_azim;
//reset change in elevation and roll of the camera;
dt_elev = dt_azim = 0.0;
CameraMode = kINACTIVE;
} else if (state == GLUT_DOWN) {
MouseStartX = MousePrevX = x;
MouseStartY = MousePrevY = y;
switch(button) {
case GLUT_LEFT_BUTTON:
//rotating
CameraMode = kROTATE;
break;
case GLUT_MIDDLE_BUTTON:
// translating
CameraMode = kTRANSLATE;
glGetIntegerv(GL_VIEWPORT, ViewPort);
glGetDoublev(GL_MODELVIEW_MATRIX, MvMat.GetPtr());
glGetDoublev(GL_PROJECTION_MATRIX, ProjMat.GetPtr());
// height of window in pixels
realy = ViewPort[3] - y - 1;
gluProject(aim_.x(), aim_.y(), aim_.z(), MvMat.GetPtr(),
ProjMat.GetPtr(), ViewPort, &wx, &wy, &wz);
gluUnProject((GLdouble) x, (GLdouble) realy, wz, MvMat.GetPtr(),
ProjMat.GetPtr(), ViewPort, &PrevMousePos.x(),
&PrevMousePos.y(), &PrevMousePos.z());
break;
case GLUT_RIGHT_BUTTON:
CameraMode = kZOOM;
break;
}
}
}
void drawBlock( Uint16 shape, Uint8 orientation, Uint8 ix, Uint8 iy, Uint8 iz ){
Mat3d rotmat = rotations[ orientation & 0b00011111 ];
rotmat.a.mul( scaling.x );
rotmat.b.mul( scaling.y );
rotmat.c.mul( scaling.z );
if (orientation & 0b10000000) rotmat.a.mul( -1.0d );
if (orientation & 0b01000000) rotmat.b.mul( -1.0d );
if (orientation & 0b00100000) rotmat.c.mul( -1.0d );
Vec3d pos; pos.set( ix*scaling.x+pos0.x, iy*scaling.y+pos0.y, iz*scaling.z+pos0.z );
float glMat[16];
glPushMatrix ( );
/*
printf( " pos (%3.3f,%3.3f,%3.3f) \n", pos.x, pos.y, pos.z );
printf( " a (%3.3f,%3.3f,%3.3f) \n", rotmat.a.x, rotmat.a.x, rotmat.a.x );
printf( " b (%3.3f,%3.3f,%3.3f) \n", rotmat.b.x, rotmat.b.x, rotmat.b.x );
printf( " c (%3.3f,%3.3f,%3.3f) \n", rotmat.c.x, rotmat.c.x, rotmat.c.x );
*/
Draw3D::toGLMat( pos, rotmat, glMat );
glMultMatrixf( glMat );
glCallList ( shape );
glPopMatrix ( );
}
void setupBlockWorld( const Vec3d& pos0_, const Vec3d& scaling_, const Mat3d& rot ){
pos0 .set( pos0_ );
scaling.set( scaling_ );
buildRotations( rot );
/*
printf( " pos (%3.3f,%3.3f,%3.3f) \n", pos0.x, pos0.y, pos0.z );
//printf( " scaling_ (%3.3f,%3.3f,%3.3f) \n", scaling_.x, scaling_.y, scaling_.z );
printf( " scaling (%3.3f,%3.3f,%3.3f) \n", scaling.x, scaling.y, scaling.z );
printf( " a (%3.3f,%3.3f,%3.3f) \n", rotations[0].a.x, rotations[0].a.y, rotations[0].a.z );
printf( " b (%3.3f,%3.3f,%3.3f) \n", rotations[0].b.x, rotations[0].b.y, rotations[0].b.z );
printf( " c (%3.3f,%3.3f,%3.3f) \n", rotations[0].c.x, rotations[0].c.y, rotations[0].c.z );
*/
}
void APFSDS::drawtra(WarDraw *wd){
/* cull object */
if(wd->vw->gc->cullFrustum(pos, getHitRadius()))
return;
double pixels = getHitRadius() * fabs(wd->vw->gc->scale(pos));
if(pixels < .1)
return;
/* bullet glow */
{
Vec3d dv = (pos - wd->vw->pos).norm();
Vec3d forward = velo.norm();
GLubyte a[4] = {255,255,127, (GLubyte)(128. * (forward.sp(dv) + 1.) / 2.)};
if(2 < a[3]){
glPushAttrib(GL_LIGHTING_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
glDepthMask(0);
gldSpriteGlow(pos, 10., a, wd->vw->irot);
glPopAttrib();
}
}
}
void scene::addText(const std::string & l, unsigned short color, Vec3d & point, osgText::Text *text)
{
dxfLayer* layer = _layerTable->findOrCreateLayer(l);
if (layer->getFrozen()) return;
sceneLayer* ly = findOrCreateSceneLayer(l);
// Apply the scene settings to the text size and rotation
Vec3d pscene(addVertex(point));
Vec3d xvec = addVertex( point + (text->getRotation() * X_AXIS) ) - pscene;
Vec3d yvec = addVertex( point + (text->getRotation() * Y_AXIS) ) - pscene;
text->setCharacterSize( text->getCharacterHeight()*yvec.length(), text->getCharacterAspectRatio()*yvec.length()/xvec.length() );
Matrix qm = _r * _m;
Vec3d t, s;
Quat ro, so;
qm.decompose( t, ro, s, so );
text->setRotation( text->getRotation() * ro );
sceneLayer::textInfo ti( correctedColorIndex(l,color), pscene, text );
ly->_textList.push_back(ti);
}
void referencetransform :: FromPlain (const Point3d & pp, Point3d & p) const
{
Vec3d v;
// v = (h * pp.X()) * ex + (h * pp.Y()) * ey + (h * pp.Z()) * ez;
// p = rp + v;
v.X() = pp.X() * exh.X() + pp.Y() * eyh.X() + pp.Z() * ezh.X();
v.Y() = pp.X() * exh.Y() + pp.Y() * eyh.Y() + pp.Z() * ezh.Y();
v.Z() = pp.X() * exh.Z() + pp.Y() * eyh.Z() + pp.Z() * ezh.Z();
p.X() = rp.X() + v.X();
p.Y() = rp.Y() + v.Y();
p.Z() = rp.Z() + v.Z();
}
Vec3d PointLight::shadowAttenuation(const Vec3d& P) const
{
// YOUR CODE HERE:
// You should implement shadow-handling code here.
Vec3d direction = getDirection(P);
ray r(P, direction, ray::SHADOW );
isect i;
if(scene->intersect( r, i )){
Vec3d iposition = r.at(i.t);
Vec3d iray = iposition - P;
Vec3d lightray = position - P;
double dlight = lightray.length();
double diray = iray.length();
if(diray > dlight)
return Vec3d(1,1,1);
else
return i.getMaterial().kt(i);
}
else {
return Vec3d(1,1,1);
}
}
void Vec3d :: GetNormal (Vec3d & n) const
{
if (fabs (X()) > fabs (Z()))
{
n.X() = -Y();
n.Y() = X();
n.Z() = 0;
}
else
{
n.X() = 0;
n.Y() = Z();
n.Z() = -Y();
}
double len = n.Length();
if (len == 0)
{
n.X() = 1;
n.Y() = n.Z() = 0;
}
else
n /= len;
}
void MassPointSystem::buildDoubleTetrahedron(const Vec3d ¢er, const Vec3d &vel, const Vec3d &angVel)
{
massPointVector.push_back(MassPoint(center + Vec3d(0.0, 0.0, 4.082), vel + angVel.cross(Vec3d(0.0, 0.0, 4.082))));
massPointVector.push_back(MassPoint(center + Vec3d(-1.443, -2.5, 0.0), vel + angVel.cross(Vec3d(-1.443, -2.5, 0.0))));
massPointVector.push_back(MassPoint(center + Vec3d(-1.443, 2.5, 0.0), vel + angVel.cross(Vec3d(-1.443, 2.5, 0.0))));
massPointVector.push_back(MassPoint(center + Vec3d(2.886, 0.0, 0.0), vel + angVel.cross(Vec3d(2.886, 0.0, 0.0))));
massPointVector.push_back(MassPoint(center + Vec3d(0.0, 0.0, -4.082), vel + angVel.cross(Vec3d(0.0, 0.0, -4.082))));
jointVector.push_back(new SpringDamperJoint(0, 1, &massPointVector));
jointVector.push_back(new SpringDamperJoint(0, 2, &massPointVector));
jointVector.push_back(new SpringDamperJoint(0, 3, &massPointVector));
jointVector.push_back(new SpringDamperJoint(1, 2, &massPointVector));
jointVector.push_back(new SpringDamperJoint(1, 3, &massPointVector));
jointVector.push_back(new SpringDamperJoint(1, 4, &massPointVector));
jointVector.push_back(new SpringDamperJoint(2, 3, &massPointVector));
jointVector.push_back(new SpringDamperJoint(2, 4, &massPointVector));
jointVector.push_back(new SpringDamperJoint(3, 4, &massPointVector));
for (unsigned int i = 0; i < massPointVector.size(); ++i)
{
jointVector.push_back(new GroundContactJoint(i));
jointVector.push_back(new GravityJoint(i));
}
}
Vec4d Raytracer::shade(const RayIntersection &intersection,
size_t depth) const {
// This offset must be added to intersection points for further
// traced rays to avoid noise in the image
const Vec3d offset(intersection.normal() * Math::safetyEps());
Vec4d color(0, 0, 0, 1);
std::shared_ptr<const Renderable> renderable = intersection.renderable();
std::shared_ptr<const Material> material = renderable->material();
for (size_t i = 0; i < mScene->lights().size(); ++i) {
const Light &light = *(mScene->lights()[i].get());
//Shadow ray from light to hit point.
const Vec3d L = (intersection.position() + offset) - light.position();
const Ray shadowRay(light.position(), L);
//Shade only if light in visible from intersection point.
if (!mScene->anyIntersection(shadowRay, L.length()))
color += material->shade(intersection, light);
}
return color;
}
void ParticleSystem::computeForces( LargeVector<Matx33d> &K )
{
K.resize(pts.size());
K.clear();
for (size_t i = 0; i < pts.size(); i++)
{
pts[i].f = pts[i].m * g + pts[i].fu + airDamping * pts[i].v;
}
for (size_t i = 0; i < springs.size(); i++)
{
int p1 = springs[i].p1;
int p2 = springs[i].p2;
double l0 = springs[i].l0;
double ks = springs[i].ks;
double kd = springs[i].kd;
Vec3d dx = pts[p1].cp - pts[p2].cp;
Vec3d dv = pts[p1].v - pts[p2].v;
double d2 = dx.ddot(dx);
double d = sqrt(d2);
Vec3d dxdir = dx / d;
Matx31d tmpDx = Matx31d(dx);
Matx33d tmpK = ks * (-I33d + l0 / d *
(I33d - 1 / d2 * tmpDx * tmpDx.t()));
K[p1] += tmpK;
K[p2] += tmpK;
Vec3d f = -ks * (d - l0) * dxdir
+ kd * dv.ddot(dxdir) * dxdir;
pts[p1].f += f;
pts[p2].f -= f;
}
}
bool RayIntersector::intersects(const BoundingSphere& bs)
{
// if bs not valid then return false based on the assumption that the node is empty.
if (!bs.valid()) return false;
// test for _start inside the bounding sphere
Vec3d sm = _start - bs._center;
double c = sm.length2() - bs._radius * bs._radius;
if (c<0.0) return true;
// solve quadratic equation
double a = _direction.length2();
double b = (sm * _direction) * 2.0;
double d = b * b - 4.0 * a * c;
// no intersections if d<0
if (d<0.0) return false;
// compute two solutions of quadratic equation
d = sqrt(d);
double div = 1.0/(2.0*a);
double r1 = (-b-d)*div;
double r2 = (-b+d)*div;
// return false if both intersections are before the ray start
if (r1<=0.0 && r2<=0.0) return false;
// if LIMIT_NEAREST and closest point of bounding sphere is further than already found intersection, return false
if (_intersectionLimit == LIMIT_NEAREST && !getIntersections().empty())
{
double minDistance = sm.length() - bs._radius;
if (minDistance >= getIntersections().begin()->distance) return false;
}
// passed all the rejection tests so line must intersect bounding sphere, return true.
return true;
}
void TerrainManipulator::clampOrientation()
{
if (_rotationMode==ELEVATION_HEADING)
{
osg::Matrixd rotation_matrix;
rotation_matrix.makeRotate(_rotation);
osg::Vec3d lookVector = -getUpVector(rotation_matrix);
osg::Vec3d upVector = getFrontVector(rotation_matrix);
CoordinateFrame coordinateFrame = getCoordinateFrame(_center);
osg::Vec3d localUp = getUpVector(coordinateFrame);
//osg::Vec3d localUp = _previousUp;
osg::Vec3d sideVector = lookVector ^ localUp;
if (sideVector.length()<0.1)
{
OSG_NOTIFY(osg::INFO)<<"Side vector short "<<sideVector.length()<<std::endl;
sideVector = upVector^localUp;
sideVector.normalize();
}
Vec3d newUpVector = sideVector^lookVector;
newUpVector.normalize();
osg::Quat rotate_roll;
rotate_roll.makeRotate(upVector,newUpVector);
if (!rotate_roll.zeroRotation())
{
_rotation = _rotation * rotate_roll;
}
}
}
