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 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);
}