//==============================================================================
bool rcbSphere::intersection(
const rcbLine3D& a_line,
rcbVector3D& a_vc_out1,
rcbVector3D& a_vc_out2
) const
//
// true if intersection found
//
{
const rcbUnitVector3D& uvc = a_line.get_vector_along();
double x = 1;
double z = 0;
double y = 0;
if (!is_zero_dbl(uvc.getY()))
{
z = 1;
y = - (uvc.getZ() / uvc.getY()) - uvc.getX();
}
else if (!is_zero_dbl(uvc.getZ()))
{
y = 1;
z = - (uvc.getY() / uvc.getZ()) - uvc.getX();
}
else if (!is_zero_dbl(uvc.getX()))
{
x = 0;
y = 1;
z = 1;
}
else { assert(0); }
rcbUnitVector3D uvc_aux(x, y, z);
rcbUnitVector3D uvc_norm1(uvc.vector_mul(uvc_aux));
rcbUnitVector3D uvc_norm2(uvc_norm1.vector_mul(uvc));
rcbPlane plane1(uvc_norm1, a_line.get_point_on_line());
rcbPlane plane2(uvc_norm2, a_line.get_point_on_line());
return solve_sphere_plane_plane_system(
m_centre, m_radius,
plane1.get_norm(), plane1.get_free_coef(),
plane2.get_norm(), plane2.get_free_coef(),
a_vc_out1, a_vc_out2
);
}
bool dgCollisionConvexHull::CheckConvex (dgPolyhedra& polyhedra1, const dgBigVector* hullVertexArray) const
{
dgPolyhedra polyhedra(polyhedra1);
dgPolyhedra::Iterator iter (polyhedra);
dgBigVector center (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgInt32 count = 0;
dgInt32 mark = polyhedra.IncLRU();
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
if (edge->m_mark < mark) {
count ++;
center += hullVertexArray[edge->m_incidentVertex];
dgEdge* ptr = edge;
do {
ptr->m_mark = mark;
ptr = ptr->m_twin->m_next;
} while (ptr != edge);
}
}
center = center.Scale3 (dgFloat64 (1.0f) / dgFloat64 (count));
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
dgBigVector normal0 (FaceNormal (edge, hullVertexArray));
dgBigVector normal1 (FaceNormal (edge->m_twin, hullVertexArray));
dgBigPlane plane0 (normal0, - (normal0 % hullVertexArray[edge->m_incidentVertex]));
dgBigPlane plane1 (normal1, - (normal1 % hullVertexArray[edge->m_twin->m_incidentVertex]));
dgFloat64 test0 = plane0.Evalue(center);
if (test0 > dgFloat64 (1.0e-3f)) {
return false;
}
dgFloat64 test1 = plane1.Evalue(center);
// if (test1 > dgFloat64 (0.0f)) {
if (test1 > dgFloat64 (1.0e-3f)) {
return false;
}
}
return true;
}
bool ON_Circle::Create( // circle through three 3d points
const ON_3dPoint& P,
const ON_3dPoint& Q,
const ON_3dPoint& R
)
{
ON_3dPoint C;
ON_3dVector X, Y, Z;
// return ( radius > 0.0 && plane.IsValid() );
//m_point[0] = P;
//m_point[1] = Q;
//m_point[2] = R;
// get normal
bool rc = Z.PerpendicularTo( P, Q, R );
// get center as the intersection of 3 planes
//
ON_Plane plane0( P, Z );
ON_Plane plane1( 0.5*(P+Q), P-Q );
ON_Plane plane2( 0.5*(R+Q), R-Q );
if ( !ON_Intersect( plane0, plane1, plane2, C ) )
rc = false;
X = P - C;
radius = X.Length();
if ( radius == 0.0 )
rc = false;
X.Unitize();
Y = ON_CrossProduct( Z, X );
Y.Unitize();
plane.origin = C;
plane.xaxis = X;
plane.yaxis = Y;
plane.zaxis = Z;
plane.UpdateEquation();
return rc;
}
void Plane::Test()
{
BoundingSphere sphere1(Vector3f(0.0f, 0.0f, 0.0f), 1.0f);
BoundingSphere sphere2(Vector3f(0.0f, 3.0f, 0.0f), 1.0f);
BoundingSphere sphere3(Vector3f(0.0f, 0.0f, 2.0f), 1.0f);
BoundingSphere sphere4(Vector3f(1.0f, 0.0f, 0.0f), 1.0f);
Plane plane1(Vector3f(0.0f, 1.0f, 0.0f), 0.0f);
IntersectData plane1IntersectSphere1 = plane1.IntersectSphere(sphere1);
IntersectData plane1IntersectSphere2 = plane1.IntersectSphere(sphere2);
IntersectData plane1IntersectSphere3 = plane1.IntersectSphere(sphere3);
IntersectData plane1IntersectSphere4 = plane1.IntersectSphere(sphere4);
assert(plane1IntersectSphere1.GetDoesIntersect() == true);
assert(plane1IntersectSphere1.GetDistance() == 1.0f);
assert(plane1IntersectSphere2.GetDoesIntersect() == false);
assert(plane1IntersectSphere2.GetDistance() == 2.0f);
assert(plane1IntersectSphere3.GetDoesIntersect() == true);
assert(plane1IntersectSphere3.GetDistance() == 1.0f);
assert(plane1IntersectSphere4.GetDoesIntersect() == true);
assert(plane1IntersectSphere4.GetDistance() == 1.0f);
// std::cout << "Plane1 intersect Sphere1: " << plane1IntersectSphere1.GetDoesIntersect()
// << ", Distance: " << plane1IntersectSphere1.GetDistance() << std::endl;
//
// std::cout << "Plane1 intersect Sphere2: " << plane1IntersectSphere2.GetDoesIntersect()
// << ", Distance: " << plane1IntersectSphere2.GetDistance() << std::endl;
//
// std::cout << "Plane1 intersect Sphere3: " << plane1IntersectSphere3.GetDoesIntersect()
// << ", Distance: " << plane1IntersectSphere3.GetDistance() << std::endl;
//
// std::cout << "Plane1 intersect Sphere4: " << plane1IntersectSphere4.GetDoesIntersect()
// << ", Distance: " << plane1IntersectSphere4.GetDistance() << std::endl;
}
BaseIF* makePlate(const Real& height,
const Real& thick,
const Real& radius,
const int& doHoles,
const Real& holeRadius,
const Real& holeSpace)
{
RealVect zero(D_DECL(0.0,0.0,0.0));
RealVect xAxis(D_DECL(1.0,0.0,0.0));
bool inside = true;
// Create the plate without holes
Vector<BaseIF*> pieces;
RealVect normal1(D_DECL(1.0,0.0,0.0));
RealVect point1(D_DECL(height,0.0,0.0));
PlaneIF plane1(normal1,point1,inside);
pieces.push_back(&plane1);
RealVect normal2(D_DECL(-1.0,0.0,0.0));
RealVect point2(D_DECL(height+thick,0.0,0.0));
PlaneIF plane2(normal2,point2,inside);
pieces.push_back(&plane2);
TiltedCylinderIF middle(radius,xAxis,zero,inside);
pieces.push_back(&middle);
IntersectionIF plate(pieces);
// Make the drills
Vector<BaseIF*> drillBits;
// Compute how many drills are needed in each direciton - 2*num+1 -
// conservatively
int num = (int)((radius - holeRadius) / holeSpace + 1.0);
if (doHoles != 0)
{
for (int i = -num; i <= num; i++)
{
for (int j = -num; j <= num; j++)
{
RealVect center(D_DECL(0.0,i*holeSpace,j*holeSpace));
TiltedCylinderIF* drill = new TiltedCylinderIF(holeRadius,xAxis,center,inside);
drillBits.push_back(drill);
}
}
}
UnionIF drills(drillBits);
ComplementIF notDrills(drills,true);
// Drill the plate
IntersectionIF* holyPlate = new IntersectionIF(plate,notDrills);
return holyPlate;
}
BaseIF* makeVane(const Real& thick,
const RealVect& normal,
const Real& innerRadius,
const Real& outerRadius,
const Real& offset,
const Real& height,
const Real& angle)
{
RealVect zeroVect(D_DECL(0.0,0.0,0.0));
RealVect xAxis(D_DECL(1.0,0.0,0.0));
bool inside = true;
Vector<BaseIF*> vaneParts;
Real sinTheta = sin(angle);
#if CH_SPACEDIM == 3
Real cosTheta = cos(angle);
// Each side of the vane (infinite)
// rotate the normal around x-axis
RealVect normal1(D_DECL(normal[0],cosTheta*normal[1]-sinTheta*normal[2],sinTheta*normal[1]+cosTheta*normal[2]));
// rotate point on top of vane around x-axis
RealVect point(D_DECL(offset+height/2.0,-thick/2.0,0.0));
RealVect point1(D_DECL(point[0],cosTheta*point[1]-sinTheta*point[2],sinTheta*point[1]+cosTheta*point[2]));
PlaneIF plane1(normal1,point1,inside);
vaneParts.push_back(&plane1);
RealVect normal2(-normal1);
// rotate point on bottom (-point[2] of vane around x-axis
RealVect point2(D_DECL(point[0],-cosTheta*point[1]-sinTheta*point[2],-sinTheta*point[1]+cosTheta*point[2]));
PlaneIF plane2(normal2,point2,inside);
vaneParts.push_back(&plane2);
#endif
// Make sure we only get something to the right of the origin
RealVect normal3(D_DECL(0.0,-sinTheta,cosTheta));
RealVect point3(D_DECL(0.0,0.0,0.0));
PlaneIF plane3(normal3,point3,inside);
vaneParts.push_back(&plane3);
// Cut off the top and bottom
RealVect normal4(D_DECL(1.0,0.0,0.0));
RealVect point4(D_DECL(offset,0.0,0.0));
PlaneIF plane4(normal4,point4,inside);
vaneParts.push_back(&plane4);
RealVect normal5(D_DECL(-1.0,0.0,0.0));
RealVect point5(D_DECL(offset+height,0.0,0.0));
PlaneIF plane5(normal5,point5,inside);
vaneParts.push_back(&plane5);
// The outside of the inner cylinder
TiltedCylinderIF inner(innerRadius,xAxis,zeroVect,!inside);
vaneParts.push_back(&inner);
// The inside of the outer cylinder
TiltedCylinderIF outer(outerRadius,xAxis,zeroVect,inside);
vaneParts.push_back(&outer);
IntersectionIF* vane = new IntersectionIF(vaneParts);
return vane;
}
/**
* Triangulates faceB using edges of faceA that both are complanars.
* @param mesh mesh that contains the faces, edges and vertices
* @param facesB set of faces from object B
* @param faceA face from object A
* @param faceB face from object B
* @param invert indicates if faceA has priority over faceB
*/
void BOP_intersectCoplanarFaces(BOP_Mesh* mesh,
BOP_Faces* facesB,
BOP_Face* faceA,
BOP_Face* faceB,
bool invert)
{
unsigned int oldSize = facesB->size();
unsigned int originalFaceB = faceB->getOriginalFace();
MT_Point3 p1 = mesh->getVertex(faceA->getVertex(0))->getPoint();
MT_Point3 p2 = mesh->getVertex(faceA->getVertex(1))->getPoint();
MT_Point3 p3 = mesh->getVertex(faceA->getVertex(2))->getPoint();
MT_Vector3 normal(faceA->getPlane().x(),faceA->getPlane().y(),faceA->getPlane().z());
MT_Vector3 p1p2 = p2-p1;
MT_Plane3 plane1((p1p2.cross(normal).normalized()),p1);
BOP_Segment sA;
sA.m_cfg1 = BOP_Segment::createVertexCfg(1);
sA.m_v1 = faceA->getVertex(0);
sA.m_cfg2 = BOP_Segment::createVertexCfg(2);
sA.m_v2 = faceA->getVertex(1);
BOP_intersectCoplanarFaces(mesh,facesB,faceB,sA,plane1,invert);
MT_Vector3 p2p3 = p3-p2;
MT_Plane3 plane2((p2p3.cross(normal).normalized()),p2);
sA.m_cfg1 = BOP_Segment::createVertexCfg(2);
sA.m_v1 = faceA->getVertex(1);
sA.m_cfg2 = BOP_Segment::createVertexCfg(3);
sA.m_v2 = faceA->getVertex(2);
if (faceB->getTAG() == BROKEN) {
for(unsigned int idxFace = oldSize; idxFace < facesB->size(); idxFace++) {
BOP_Face *face = (*facesB)[idxFace];
if (face->getTAG() != BROKEN && originalFaceB == face->getOriginalFace())
BOP_intersectCoplanarFaces(mesh,facesB,face,sA,plane2,invert);
}
}
else {
BOP_intersectCoplanarFaces(mesh,facesB,faceB,sA,plane2,invert);
}
MT_Vector3 p3p1 = p1-p3;
MT_Plane3 plane3((p3p1.cross(normal).safe_normalized()),p3);
sA.m_cfg1 = BOP_Segment::createVertexCfg(3);
sA.m_v1 = faceA->getVertex(2);
sA.m_cfg2 = BOP_Segment::createVertexCfg(1);
sA.m_v2 = faceA->getVertex(0);
if (faceB->getTAG() == BROKEN) {
for(unsigned int idxFace = oldSize; idxFace < facesB->size(); idxFace++) {
BOP_Face *face = (*facesB)[idxFace];
if (face->getTAG() != BROKEN && originalFaceB == face->getOriginalFace())
BOP_intersectCoplanarFaces(mesh,facesB,face,sA,plane3,invert);
}
}
else {
BOP_intersectCoplanarFaces(mesh,facesB,faceB,sA,plane3,invert);
}
}
int main()
try {
Point tl(300,50);
Simple_window win(tl,1000,800,"My window");
win.wait_for_button();
win.set_label("My window");
// add grid on leftmost 800-by-800 part
Lines grid;
int x_size = 800;
int y_size = 800;
for (int i = 100; i<=x_size; i+=100) {
grid.add(Point(i,0),Point(i,y_size));
grid.add(Point(0,i),Point(x_size,i));
}
win.attach(grid);
//win.resize(1000,800);
win.wait_for_button();
// make squares on the diagonal red
Vector_ref<Graph_lib::Rectangle> vr;
for (int i = 0; i<8; ++i) {
vr.push_back(new Graph_lib::Rectangle(Point(i*100,i*100),101,101));
vr[vr.size()-1].set_fill_color(Color::red);
win.attach(vr[vr.size()-1]);
}
//win.resize(1000,800);
win.wait_for_button();
// place 3 copies of a 200-by-200 image, don't cover the red squares
Image plane1(Point(200,0),"pics_and_txt/image.jpg");
plane1.set_mask(Point(200,0),200,200);
win.attach(plane1);
Image plane2(Point(500,200),"pics_and_txt/image.jpg");
plane2.set_mask(Point(200,0),200,200);
win.attach(plane2);
Image plane3(Point(100,500),"pics_and_txt/image.jpg");
plane3.set_mask(Point(200,0),200,200);
win.attach(plane3);
//win.resize(1000,800);
win.wait_for_button();
// add a 100-by-100 image, have it move around
Image snow(Point(0,0),"pics_and_txt/snow_cpp.gif");
snow.set_mask(Point(110,70),100,100);
win.attach(snow);
//win.resize(1000,800);
win.wait_for_button();
int x = 0;
int y = 0;
int dx = 0;
int dy = 0;
while (true) {
x = randint(8);
y = randint(8);
dx = 100*x - snow.point(0).x;
dy = 100*y - snow.point(0).y;
snow.move(dx,dy);
//win.resize(1000,800);
win.wait_for_button();
}
}
catch (exception& e) {
cerr << "exception: " << e.what() << endl;
keep_window_open();
}
catch (...) {
cerr << "exception\n";
keep_window_open();
}
int
main(int argc, const char ** argv)
{
bool opt_display = true;
bool opt_click_allowed = true;
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
exit (-1);
}
vpImage<unsigned char> I(512,512,0) ;
// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
vpDisplayGTK display;
#elif defined VISP_HAVE_GDI
vpDisplayGDI display;
#endif
if (opt_display) {
try{
// Display size is automatically defined by the image (I) size
display.init(I, 100, 100,"Camera view...") ;
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
vpDisplay::display(I) ;
vpDisplay::flush(I) ;
}
catch(...)
{
vpERROR_TRACE("Error while displaying the image") ;
exit(-1);
}
}
double px, py ; px = py = 600 ;
double u0, v0 ; u0 = v0 = 256 ;
vpCameraParameters cam(px,py,u0,v0);
vpServo task ;
vpSimulatorCamera robot ;
// sets the initial camera location
vpHomogeneousMatrix cMo(-0.2,0.1,1,
vpMath::rad(5), vpMath::rad(5), vpMath::rad(90));
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
robot.getPosition(wMc) ;
wMo = wMc * cMo;
// sets the final camera location (for simulation purpose)
vpHomogeneousMatrix cMod(0,0,1,
vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
// sets the line coordinates (2 planes) in the world frame
vpColVector plane1(4) ;
vpColVector plane2(4) ;
plane1[0] = 0; // z = 0
plane1[1] = 0;
plane1[2] = 1;
plane1[3] = 0;
plane2[0] = 0; // y =0
plane2[1] = 1;
plane2[2] = 0;
plane2[3] = 0;
vpLine line ;
line.setWorldCoordinates(plane1, plane2) ;
// sets the desired position of the visual feature
line.track(cMod) ;
line.print() ;
vpFeatureLine ld ;
vpFeatureBuilder::create(ld,line) ;
// computes the line coordinates in the camera frame and its 2D coordinates
// sets the current position of the visual feature
line.track(cMo) ;
line.print() ;
vpFeatureLine l ;
vpFeatureBuilder::create(l,line) ;
l.print() ;
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA) ;
// we want to see a line on a line
task.addFeature(l,ld) ;
vpDisplay::display(I) ;
vpServoDisplay::display(task,cam,I) ;
vpDisplay::flush(I) ;
// set the gain
task.setLambda(1) ;
// Display task information " ) ;
task.print() ;
if (opt_display && opt_click_allowed) {
std::cout << "\n\nClick in the camera view window to start..." << std::endl;
vpDisplay::getClick(I) ;
}
unsigned int iter=0 ;
// loop
while(iter++<200)
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
vpColVector v ;
// get the robot position
robot.getPosition(wMc) ;
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new line position
line.track(cMo) ;
// retrieve x,y and Z of the vpLine structure
vpFeatureBuilder::create(l,line);
if (opt_display) {
vpDisplay::display(I) ;
vpServoDisplay::display(task,cam,I) ;
vpDisplay::flush(I) ;
}
// compute the control law
v = task.computeControlLaw() ;
// send the camera velocity to the controller
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ;
}
if (opt_display && opt_click_allowed) {
std::cout << "\nClick in the camera view window to end..." << std::endl;
vpDisplay::getClick(I) ;
}
// Display task information
task.print() ;
task.kill();
}
void GLBasicShadowMapRenderer::BuildMatrix(float near, float far){
// TODO: variable light direction?
Vector3 lightDir = MakeVector3(0, -1, -1).Normalize();
// set better up dir?
Vector3 up = MakeVector3(0, 0, 1);
Vector3 side = Vector3::Cross(up, lightDir).Normalize();
up = Vector3::Cross(lightDir, side).Normalize();
// build frustrum
client::SceneDefinition def = GetRenderer()->GetSceneDef();
Vector3 frustrum[8];
float tanX = tanf(def.fovX * .5f);
float tanY = tanf(def.fovY * .5f);
frustrum[0] = FrustrumCoord(def, tanX, tanY, near);
frustrum[1] = FrustrumCoord(def, tanX, -tanY, near);
frustrum[2] = FrustrumCoord(def, -tanX, tanY, near);
frustrum[3] = FrustrumCoord(def, -tanX, -tanY, near);
frustrum[4] = FrustrumCoord(def, tanX, tanY, far);
frustrum[5] = FrustrumCoord(def, tanX, -tanY, far);
frustrum[6] = FrustrumCoord(def, -tanX, tanY, far);
frustrum[7] = FrustrumCoord(def, -tanX, -tanY, far);
// compute frustrum's x,y boundary
float minX, maxX, minY, maxY;
minX = maxX = Vector3::Dot(frustrum[0], side);
minY = maxY = Vector3::Dot(frustrum[0], up);
for(int i = 1; i < 8; i++){
float x = Vector3::Dot(frustrum[i], side);
float y = Vector3::Dot(frustrum[i], up);
if(x < minX) minX = x;
if(x > maxX) maxX = x;
if(y < minY) minY = y;
if(y > maxY) maxY = y;
}
// compute frustrum's z boundary
Segment seg;
Plane3 plane1(0,0,1,-4.f);
Plane3 plane2(0,0,1,64.f);
seg += ZRange(side * minX + up * minY,
lightDir, plane1, plane2);
seg += ZRange(side * minX + up * maxY,
lightDir, plane1, plane2);
seg += ZRange(side * maxX + up * minY,
lightDir, plane1, plane2);
seg += ZRange(side * maxX + up * maxY,
lightDir, plane1, plane2);
for(int i = 1; i < 8; i++){
seg += Vector3::Dot(frustrum[i], lightDir);
}
// build frustrum obb
Vector3 origin = side * minX + up * minY + lightDir * seg.low;
Vector3 axis1 = side * (maxX - minX);
Vector3 axis2 = up * (maxY - minY);
Vector3 axis3 = lightDir * (seg.high - seg.low);
obb = OBB3(Matrix4::FromAxis(axis1, axis2, axis3,
origin));
vpWidth = 2.f / axis1.GetLength();
vpHeight = 2.f / axis2.GetLength();
// convert to projectionview matrix
matrix = obb.m.InversedFast();
matrix = Matrix4::Scale(2.f) * matrix;
matrix = Matrix4::Translate(-1, -1, -1) * matrix;
// scale a little big for padding
matrix = Matrix4::Scale(.98f) * matrix;
//
matrix = Matrix4::Scale(1,1,-1) * matrix;
// make sure frustrums in range
#ifndef NDEBUG
for(int i = 0; i < 8; i++){
Vector4 v = matrix * frustrum[i];
SPAssert(v.x >= -1.f);
SPAssert(v.y >= -1.f);
//SPAssert(v.z >= -1.f);
SPAssert(v.x < 1.f);
SPAssert(v.y < 1.f);
//SPAssert(v.z < 1.f);
}
#endif
}
