void gothic_arch(double width, double height, double depth)
{
double radius;
double archheight;
double cubeheight;
if (height < width * 2.0)
width = height / 2.0;
radius = 5.0 * width / 4.0;
archheight = width;
cubeheight = height - archheight;
xlate(-depth / 2.0, 0, cubeheight);
onion();
intersection();
xlate(0, 3.0 * width / 4.0, 0);
rotate(90, 0, 1, 0);
cylinder(depth, radius, radius);
endrotate();
endxlate();
xlate(0, -3.0 * width / 4.0, 0);
rotate(90, 0, 1, 0);
cylinder(depth, radius, radius);
endrotate();
endxlate();
endintersection();
xlate(depth / 2.0, 0, -cubeheight / 2.0);
cube(depth, width, cubeheight, 1);
endxlate();
endonion();
endxlate();
}
void CylindricalMapping2D<Conf>::mappingImpl(
const typename Conf::differential_geometry_t& dg,
typename Conf::scalar_t& s,
typename Conf::scalar_t& t,
typename Conf::scalar_t& dsdx,
typename Conf::scalar_t& dtdx,
typename Conf::scalar_t& dsdy,
typename Conf::scalar_t& dtdy
)const
{
cylinder(dg.point,s,t);
scalar_t sx,tx,sy,ty;
const scalar_t delta(.01f);
const scalar_t inv_delta(100.f);
cylinder(dg.point + delta * dg.dpdx, sx,tx);
dsdx = (sx-s) * inv_delta;
dtdx = (tx - t) * inv_delta;
if (dtdx > scalar_t(0.5f)) dtdx = 1 - dtdx;
else if(dtdx < scalar_t(-0.5f)) dtdx = -(dtdx + 1);
cylinder(dg.point + delta * dg.dpdy,sy,ty);
dsdy = (sy - s ) * inv_delta;
dtdy = (ty - t) * inv_delta;
if (dtdy > scalar_t(0.5f)) dtdy = 1 - dtdy;
else if(dtdy < scalar_t(-0.5f)) dtdy = -(dtdy + 1);
}
Trade::MeshData3D Cylinder::solid(const UnsignedInt rings, const UnsignedInt segments, const Float halfLength, const Flags flags) {
CORRADE_ASSERT(rings >= 1 && segments >= 3, "Primitives::Cylinder::solid(): cylinder must have at least one ring and three segments", Trade::MeshData3D(MeshPrimitive::Triangles, {}, {}, {}, {}));
Implementation::Spheroid cylinder(segments, flags & Flag::GenerateTextureCoords ? Implementation::Spheroid::TextureCoords::Generate : Implementation::Spheroid::TextureCoords::DontGenerate);
const Float length = 2.0f*halfLength;
const Float textureCoordsV = flags & Flag::CapEnds ? 1.0f/(length+2.0f) : 0.0f;
/* Bottom cap */
if(flags & Flag::CapEnds) {
cylinder.capVertex(-halfLength, -1.0f, 0.0f);
cylinder.capVertexRing(-halfLength, textureCoordsV, Vector3::yAxis(-1.0f));
}
/* Vertex rings */
cylinder.cylinderVertexRings(rings+1, -halfLength, length/rings, textureCoordsV, length/(rings*(flags & Flag::CapEnds ? length + 2.0f : length)));
/* Top cap */
if(flags & Flag::CapEnds) {
cylinder.capVertexRing(halfLength, 1.0f - textureCoordsV, Vector3::yAxis(1.0f));
cylinder.capVertex(halfLength, 1.0f, 1.0f);
}
/* Faces */
if(flags & Flag::CapEnds) cylinder.bottomFaceRing();
cylinder.faceRings(rings, flags & Flag::CapEnds ? 1 : 0);
if(flags & Flag::CapEnds) cylinder.topFaceRing();
return cylinder.finalize();
}
void intersection(t_env *rt, t_vector ray, t_vector origin)
{
int i;
rt->i2 = -1;
i = 0;
rt->t = 200000;
rt->disk_cy = 0;
rt->disk_s = 0;
while (i < rt->nbobj)
{
rt->object[i].shadows = 0;
if ((rt->object[i].name == SPHERE && sphere(ray, rt->object[i],
&rt->t, rt->eye)) ||
(rt->object[i].name == PLANE && plane(ray, rt->object[i],
&rt->t, rt->eye)) ||
(rt->object[i].name == CYLINDER && cylinder(ray, rt->object[i],
&rt->t, rt->eye)) ||
((rt->object[i].name == CONE || rt->object[i].name == HYPERBOL)
&& cone(ray, rt->object[i],
&rt->t, rt->eye)) ||
intersection2(rt, ray, origin, i))
rt->i2 = i;
i++;
}
}
void iscrtaj(void) {
int kx = 2;
int ky = 2;
int kz = 1;
double cr = 0.1;
int cn = 20;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(0.0, -20.0, 10.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
glPushMatrix(); // postavljanje izvora svjetlosti
glTranslated(0.0, 0.0, 10.0);
svjetlo0();
glPopMatrix();
glTranslated(0.0, 0.0, -5.0);
glPushMatrix();
glRotated(kut, 0.0, 0.0, 1.0);
glTranslated(-1, -1, 0.0 + step);
kvadar(kx, ky, kz);
glTranslated(3, 5, kz);
glMaterialfv(GL_FRONT, GL_DIFFUSE, zuto);
glPopMatrix();
glEnable(GL_BLEND);
glDepthMask(GL_FALSE);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glDepthMask(GL_TRUE);
cylinder(cr, ch, cn);
glDisable(GL_BLEND);
glutSwapBuffers();
} // iscrtaj
static void arched_opening(double height, double width, double depth)
{
/* make the windows 10x too big, then scale 10x down. This
prevents small windows from having bad artifcacts due
to openscad's weird curve rendering. Judicious tweaking of
$fa, $fn, could probably accomplish the same thing, but
this seemed easier to me.
*/
height *= 10.0;
width *= 10.0;
depth *= 10.0;
scale(0.1, 0.1, 0.1);
xlate(-width / 2.0, -depth / 2.0, -height / 2.0);
onion();
cube(width, depth, height - width / 2.0, 0);
xlate(width / 2.0, depth, height - width / 2.0);
rotate(90, 1, 0, 0);
cylinder(depth, width / 2.0, width / 2.0);
endrotate();
endxlate();
endonion();
endxlate();
endscale();
}
int main(int argc,char** argv)
{
clientResetScene();
btMatrix3x3 basisA;
basisA.setIdentity();
btMatrix3x3 basisB;
basisB.setIdentity();
tr[0].setBasis(basisA);
tr[1].setBasis(basisB);
btVector3 points0[3]={btVector3(1,0,0),btVector3(0,1,0),btVector3(0,0,1)};
btVector3 points1[5]={btVector3(1,0,0),btVector3(0,1,0),btVector3(0,0,1),btVector3(0,0,-1),btVector3(-1,-1,0)};
btConvexHullShape hullA(&points0[0].getX(),3);
btConvexHullShape hullB(&points1[0].getX(),5);
btCylinderShape cylinder(btVector3(0.3,1,1));
shapePtr[0] = &cylinder;//hullA;
shapePtr[1] = &hullB;
btTransform tr;
tr.setIdentity();
return myglutmain(argc, argv,screenWidth,screenHeight,"Convex Hull Distance Demo");
}
void
Voxel::set_geometry( PyObject * distal
, PyObject * proximal
, PyObject * parent
)
{
Vec4f d(pysequence_to_vec4f(distal));
if(proximal == Py_None)
{
sphere( osg::Vec3f(d[0], d[1], d[2])
, d[3]
, node.get()
, SPHERICAL_VOXEL_POINTS
, SPHERICAL_VOXEL_COLOR
);
}
else
{
Vec4f p(pysequence_to_vec4f(proximal));
cylinder( d
, p
, node.get()
, CYLINDRICAL_VOXEL_POINTS
, CYLINDRICAL_VOXEL_COLOR
, pysequence_to_vec3f(parent)
);
}
}
// ------------------------------------------
// 台車を描画
// ------------------------------------------
void Model::truck(float theta0, float r)
{
glTranslatef(0, 0, 75);
glRotatef(theta0, 0, 0, 1);
glTranslatef(0, r, 0);
//台座
{
float Ambient[] = {0.1f, 0.1f, 0.3f, 1.0f};
float AmbientLight[] = { 1.0f, 1.0f, 0.5f, 1.0f};
glMaterialfv(GL_FRONT,GL_AMBIENT,Ambient);
glLightfv(GL_LIGHT0, GL_AMBIENT, AmbientLight);
}
cuboid(300, 400, 150);
// 車輪1
glPushMatrix();
glTranslatef(150, 0, 0);
glRotatef(90, 0, 1, 0);
glTranslatef(0, 0, 15);
glColor3f(0.5,0.5,0.5);
{
float Ambient[] = {0.2f, 0.2f, 0.2f, 1.0f};
float AmbientLight[] = { 1.0f, 1.0f, 1.0f, 1.0f};
glMaterialfv(GL_FRONT,GL_AMBIENT,Ambient);
glLightfv(GL_LIGHT0, GL_AMBIENT, AmbientLight);
}
cylinder(75, 30, 100);
glPopMatrix();
// 車輪2
glPushMatrix();
glTranslatef(-150, 0, 0);
glRotatef(-90, 0, 1, 0);
glTranslatef(0, 0, 15);
glColor3f(0.5,0.5,0.5);
{
float Ambient[] = {0.2f, 0.2f, 0.2f, 1.0f};
float AmbientLight[] = { 1.0f, 1.0f, 1.0f, 1.0f};
glMaterialfv(GL_FRONT,GL_AMBIENT,Ambient);
glLightfv(GL_LIGHT0, GL_AMBIENT, AmbientLight);
}
cylinder(75, 30, 100);
glPopMatrix();
}
void MainWindow::on_cylinderButton_released()
{
DialogCylinder cylinder(this);
if(cylinder.exec())
ui->glwidget->scene.addCylinder(cylinder.getX(), cylinder.getY(), cylinder.getZ(),
cylinder.getR(), cylinder.getH(), cylinder.getDisc() );
ui->glwidget->updateGL();
}
void GraphicsPainter::drawCylinder(float radius, float length)
{
GraphicsCylinder cylinder(radius, length);
GraphicsVertexBuffer *buffer = cylinder.tesselate(12, 10);
draw(buffer);
delete buffer;
}
int APIENTRY _tWinMain(HINSTANCE hInstance,
HINSTANCE,
LPTSTR,
int nCmdShow)
{
{ // code block
Helper::Window mainWindow( hInstance, nCmdShow, &WndProc,
L"Skinning", L"skinning", 4 );
D3DPRESENT_PARAMETERS params;
ZeroMemory( ¶ms, sizeof( params ) );
params.Windowed = TRUE;
params.SwapEffect = D3DSWAPEFFECT_DISCARD;
params.BackBufferFormat = D3DFMT_UNKNOWN;
params.EnableAutoDepthStencil = TRUE;
params.AutoDepthStencilFormat = D3DFMT_D16;
params.MultiSampleType = D3DMULTISAMPLE_NONE;
D3D::GraphicDevice graphicDevice( mainWindow.GetHWND(), params );
graphicDevice.SetRenderState( D3DRS_FILLMODE, D3DFILL_WIREFRAME );
graphicDevice.SetRenderState( D3DRS_CULLMODE, D3DCULL_NONE );
Helper::SpectatorCoords spectatorCoords( 40.0f, D3DX_PI / 2, -D3DX_PI / 2 );
Cylinder cylinder(nPointsPerCircle, nPointsPerGeneratrix, Height, Radius, graphicDevice, Freq, MaxAngle);
cylinder.SetPositionMatrix( RotateZMatrix(D3DX_PI/2)*TranslationMatrix(0, -Height/2*0, 0) );
cylinder.SetViewMatrix( ViewMatrix( spectatorCoords.GetCartesianCoords(),
D3DXVECTOR3(0.0f, 0.0f, 0.0f),
D3DXVECTOR3(0.0f, 1.0f, 0.0f)) );
cylinder.SetProjectiveMatrix( ProjectiveMatrix( FrontClippingPlane, BackClippingPlane ) );
SetWindowLong(mainWindow.GetHWND(), 0, reinterpret_cast<LONG>(&spectatorCoords));
SetWindowLong(mainWindow.GetHWND(), sizeof(LONG), reinterpret_cast<LONG>(&cylinder));
MSG msg;
ZeroMemory(&msg, sizeof(msg));
while( msg.message != WM_QUIT )
{
if( PeekMessage( &msg, NULL, 0U, 0U, PM_REMOVE ) )
{
TranslateMessage( &msg );
DispatchMessage( &msg );
}
else
{
Render(graphicDevice, spectatorCoords, cylinder);
}
}
} // code block
_CrtDumpMemoryLeaks();
return 0;
}
void CylindricalMapping2D::Map(const DifferentialGeometry &dg,
float *s, float *t, float *dsdx, float *dtdx,
float *dsdy, float *dtdy) const {
cylinder(dg.p, s, t);
// Compute texture coordinate differentials for cylinder $(u,v)$ mapping
float sx, tx, sy, ty;
const float delta = .01f;
cylinder(dg.p + delta * dg.dpdx, &sx, &tx);
*dsdx = (sx - *s) / delta;
*dtdx = (tx - *t) / delta;
if (*dtdx > .5) *dtdx = 1.f - *dtdx;
else if (*dtdx < -.5f) *dtdx = -(*dtdx + 1);
cylinder(dg.p + delta * dg.dpdy, &sy, &ty);
*dsdy = (sy - *s) / delta;
*dtdy = (ty - *t) / delta;
if (*dtdy > .5) *dtdy = 1.f - *dtdy;
else if (*dtdy < -.5f) *dtdy = -(*dtdy + 1);
}
Point2f CylindricalMapping2D::Map(const SurfaceInteraction &isect,
Vector2f *dstdx, Vector2f *dstdy) const {
Point2f st = cylinder(isect.p);
// Compute texture coordinate differentials for cylinder $(u,v)$ mapping
const Float delta = .01f;
Point2f stDeltaX = cylinder(isect.p + delta * isect.dpdx);
*dstdx = (stDeltaX - st) / delta;
if ((*dstdx)[1] > .5)
(*dstdx)[1] = 1.f - (*dstdx)[1];
else if ((*dstdx)[1] < -.5f)
(*dstdx)[1] = -((*dstdx)[1] + 1);
Point2f stDeltaY = cylinder(isect.p + delta * isect.dpdy);
*dstdy = (stDeltaY - st) / delta;
if ((*dstdy)[1] > .5)
(*dstdy)[1] = 1.f - (*dstdy)[1];
else if ((*dstdy)[1] < -.5f)
(*dstdy)[1] = -((*dstdy)[1] + 1);
return st;
}
void MpSurfacePrism (Scene &S, const Matrix &Z, int nsegments,
double wx, double wy,
const ColorMap &cmap,double cmin, double cmax)
{
// nothing that can be drawn
if ( Z.Empty() ) return;
// 4-segments faster special case
if (nsegments == 4) {
MpSurfacePrism4(S,Z,wx,wy,cmap,cmin,cmax);
return;
}
// find the extrema
double zmin = Min(Z);
// check for valid nsegments values
MpForceRange(nsegments, 3, MaxVertices);
// check if a colormap should be used
double dz,cscale = 0;
bool usecolormap;
int ncmap = cmap.Size()-1;
if (cmap) {
usecolormap = true;
dz = cmax - cmin;
cscale = (dz == 0.0) ? 0 : ncmap/dz;
} else
usecolormap = false;
// add prism
for (int x = Z.Rlo(); x <= Z.Rhi(); x++)
for (int y = Z.Clo(); y <= Z.Chi(); y++) {
double z = Z(x,y),
height = 0.5 * (z - zmin);
Trafo T = trans(y,height+zmin,x)
* scale(wy,height,wx)
* rot(Trafo::XAxis,M_PI/2);
// coloring
const ColorB *color;
if (usecolormap) {
int c = int((z-cmin)*cscale);
if (c > ncmap)
c = ncmap;
else if (c < 0)
c = 0;
color = &cmap[c];
} else
color = (ColorB*)NULL;
cylinder(S,nsegments,T,color);
}
}
void castle(double h, double r)
{
int i;
double angle, tx, ty;
diff();
cylinder(h, r, r * 0.8);
cylinder(h, 0.5 * r, 0.4 * r);
enddiff();
for (i = 0; i < 5; i++) {
angle = (PI / 180 * 360.0 / 5.0 * i);
tx = r * cos(angle);
ty = r * sin(angle);
if ((r / 2.2) >= 3.0) {
xlate(tx, ty, 0);
castle(h * 1.3, r / 2.2);
endxlate();
}
}
}
// ------------------------------------------
// 接合部を描画
// ------------------------------------------
void Model::junction(float radius, float height, float slider)
{
glPushMatrix();
glRotatef(90, 1, 0, 0);
glTranslatef(0, 0, -25);
{
float Ambient[] = {0.1f, 0.1f, 0.1f, 1.0f};
float AmbientLight[] = { 0.3f, 0.3f, 0.6f, 1.0f};
glMaterialfv(GL_FRONT,GL_AMBIENT,Ambient);
glLightfv(GL_LIGHT0, GL_AMBIENT, AmbientLight);
}
cylinder(radius, height, slider);
glPopMatrix();
}
double compute_object(t_ray ray, t_object object)
{
double len;
len = 0;
if (object.type == SPHERE)
len = sphere(ray, object);
else if (object.type == PLAN)
len = plan(ray, object);
else if (object.type == CYLINDER)
len = cylinder(ray, object);
else if (object.type == CONE)
len = cone(ray, object);
return (len);
}
t_intersect calculate_intersection(t_pov *pov, t_coordinate *rayvector,
t_object *object)
{
t_intersect intersect;
if (my_strcmp(object->type, "sphere") == 0)
intersect = sphere(*pov, *rayvector, object);
if (my_strcmp(object->type, "plane") == 0)
intersect = plane(*pov, *rayvector, object);
if (my_strcmp(object->type, "cone") == 0)
intersect = cone(*pov, *rayvector, object);
if (my_strcmp(object->type, "cylinder") == 0)
intersect = cylinder(*pov, *rayvector, object);
return (intersect);
}
Trade::MeshData3D Cylinder::wireframe(const UnsignedInt rings, const UnsignedInt segments, const Float halfLength) {
CORRADE_ASSERT(rings >= 1 && segments >= 4 && segments%4 == 0, "Primitives::Cylinder::wireframe(): improper parameters", Trade::MeshData3D(MeshPrimitive::Lines, {}, {}, {}, {}));
Implementation::WireframeSpheroid cylinder(segments/4);
const Float increment = 2*halfLength/rings;
/* Rings */
cylinder.ring(-halfLength);
for(UnsignedInt i = 0; i != rings; ++i) {
cylinder.cylinder();
cylinder.ring(-halfLength + (i+1)*increment);
}
return cylinder.finalize();
}
static void ghost(double x,double y,double z, double r, int rotate, int col)
{
// Save transformation
glPushMatrix();
// Offset and scale
glTranslated(x,y,z);
if (rotate != 0){
glRotated(rotate,0,1,0);
}
glScaled(r,r,r);
//Draw Body
sphere(x,y,z,r,rotate,col);
//draw body
glPushMatrix();
glTranslated(x,y,z);
glRotated(90,1,0,0);
if (rotate != 0){
glRotated(rotate,0,1,0);
}
glScaled(r,r,r);
cylinder(r);
// Draw eyes
// offset for right eye
glPushMatrix();
glPolygonOffset(0,0);
glTranslated(x+(r/2),y,z+r);
glScaled(r/3,r/3,r/3);
// right eye
sphere(x,y,z,r,rotate,4);
// offset for left eye
glPushMatrix();
glTranslated(x-(r/2),y,z+r);
glScaled(r/3,r/3,r/3);
//left eye
sphere(x,y,z,r,rotate,4);
}
osg::ref_ptr<osg::Node> get(const CylinderVec_t& cylinders)
{
osg::ref_ptr<osg::Geode> geode( new osg::Geode() );
for ( const auto& cc : cylinders )
{
osg::Vec3d pp( cc.begin - cc.end );
double height( pp.length() );
if ( height < 0.000001 ) continue;
osg::Vec3d center( (cc.begin.x() + cc.end.x())/2.0,
(cc.begin.y() + cc.end.y())/2.0,
(cc.begin.z() + cc.end.z())/2.0 );
// This is the default direction for the cylinders to face in OpenGL
static const osg::Vec3d ZZ( 0,0,1 );
// Get CROSS product (the axis of rotation)
osg::Vec3d tt( ZZ^pp );
// Get angle. length is magnitude of the vector
double angle( acos( (ZZ * pp) / height) );
// Create a cylinder between the two points with the given radius
osg::ref_ptr<osg::Cylinder> cylinder( new osg::Cylinder(center, cc.radius, height) );
cylinder->setRotation(osg::Quat(angle, osg::Vec3d(tt.x(), tt.y(), tt.z())));
// A geode to hold the cylinder
osg::ref_ptr<osg::ShapeDrawable> cylinderDrawable( new osg::ShapeDrawable(cylinder) );
geode->addDrawable(cylinderDrawable);
// Set the color of the cylinder that extends between the two points.
osg::ref_ptr<osg::Material> pMaterial( new osg::Material() );
pMaterial->setDiffuse( osg::Material::FRONT, cc.color);
geode->getOrCreateStateSet()->setAttribute( pMaterial, osg::StateAttribute::OVERRIDE );
geode->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
geode->getOrCreateStateSet()->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
}
osg::ref_ptr<osg::Group> pAddToThisGroup(new osg::Group());
pAddToThisGroup->addChild(geode);
return pAddToThisGroup;
}
void tower(double r, double h)
{
int i;
double h1, r1, angle, nx, ny, nz;
onion();
diff();
cylinder(h, r, r * 0.85);
cylinder(h, r * 0.85, r * 0.75);
enddiff();
xlate(0, 0, h * 0.95);
diff();
onion();
cylinder(h * 0.15, r * 0.85, r * 1.25);
xlate(0, 0, h * 0.12);
diff();
cylinder(h * 0.15, r * 1.25, r * 1.25);
cylinder(h * 0.15 + 1, r * 0.95, r * 0.95);
enddiff();
endxlate();
endonion();
xlate(0, 0, 0.25 * h);
crenelation(h, r);
endxlate();
enddiff();
endxlate();
for (i = 0; i < 5; i++) {
angle = (360.0 * rand()) / RAND_MAX * 3.1415927 / 180.0;
nx = cos(angle) * r * 0.85;
ny = sin(angle) * r * 0.85;
nz = 0.25 * h + (percent(55.0) * h);
r1 = percent(40.0) * r;
h1 = percent(50.0) * h;
if (r1 < 5)
continue;
if (h1 < 20)
continue;
xlate(nx, ny, nz);
tower(r1, h1);
endxlate();
}
xlate(0, 0, -h / 5.0);
cylinder(h / 5.0, r * 0.1, r);
endxlate();
endonion();
}
TEST(CappedCylinder, findRayIntersection) {
CappedCylinder cylinder(VECTOR_DOWN, VECTOR_UP, 1.0, 2.0);
int intersect_count;
Vector3 p1, p2;
intersect_count =
cylinder.findRayIntersection(InfiniteRay(Vector3(1.5, 0.0, 0.0), Vector3(0.0, 0.0, 1.0)), &p1, &p2);
EXPECT_EQ(0, intersect_count);
intersect_count =
cylinder.findRayIntersection(InfiniteRay(Vector3(1.0, 0.0, 0.0), Vector3(0.0, 0.0, 1.0)), &p1, &p2);
EXPECT_EQ(1, intersect_count);
EXPECT_VECTOR3_COORDS(p1, 1.0, 0.0, 0.0);
intersect_count =
cylinder.findRayIntersection(InfiniteRay(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 1.0)), &p1, &p2);
EXPECT_EQ(2, intersect_count);
EXPECT_VECTOR3_COORDS(p1, 0.5, 0.0, -cos(asin(0.5)));
EXPECT_VECTOR3_COORDS(p2, 0.5, 0.0, cos(asin(0.5)));
intersect_count =
cylinder.findRayIntersection(InfiniteRay(Vector3(0.5, -2.1, 0.0), Vector3(0.0, 0.0, 1.0)), &p1, &p2);
EXPECT_EQ(0, intersect_count);
intersect_count =
cylinder.findRayIntersection(InfiniteRay(Vector3(0.5, 2.1, 0.0), Vector3(0.0, 0.0, 1.0)), &p1, &p2);
EXPECT_EQ(0, intersect_count);
// diagonal cases (through a cap)
intersect_count =
cylinder.findRayIntersection(InfiniteRay(Vector3(-2.0, -1.0, 0.0), Vector3(1.0, 1.0, 0.0)), &p1, &p2);
EXPECT_EQ(1, intersect_count);
EXPECT_VECTOR3_COORDS(p1, -1.0, 0.0, 0.0);
intersect_count =
cylinder.findRayIntersection(InfiniteRay(Vector3(-2.0, 3.0, 0.0), Vector3(1.0, -1.0, 0.0)), &p1, &p2);
EXPECT_EQ(1, intersect_count);
EXPECT_VECTOR3_COORDS(p1, 1.0, 0.0, 0.0);
}
void PartsTest::test_Cylinder()
{
Position pos(0.0, 1.0, 2.0);
double rad = 0.1;
double len = 1.5;
CylinderParts cylinder("cylinder", pos, rad, len);
Parts &parts = cylinder;
int n;
char *data= parts.toBinary(n);
ASSERT(data != NULL);
CParts * d = CParts::decode(data+2);
ASSERT(d != NULL);
ASSERT(strcmp(d->name(), "cylinder") == 0);
ASSERT(cylinder.getType() == d->getType());
CylinderPartsCmpnt *ext = (CylinderPartsCmpnt*)( d->extdata());
ASSERT(ext != NULL);
ASSERT_D_EQUAL(rad, ext->radius(), EPS);
ASSERT_D_EQUAL(len, ext->length(), EPS);
delete d;
}
void createScenePrimitives()
{
// sphere
{
int id = numRigidBodies++;
PfxSphere sphere(1.0f);
PfxShape shape;
shape.reset();
shape.setSphere(sphere);
collidables[id].reset();
collidables[id].addShape(shape);
collidables[id].finish();
bodies[id].reset();
bodies[id].setMass(1.0f);
bodies[id].setInertia(pfxCalcInertiaSphere(1.0f,1.0f));
states[id].reset();
states[id].setPosition(PfxVector3(-5.0f,5.0f,0.0f));
states[id].setMotionType(kPfxMotionTypeActive);
states[id].setRigidBodyId(id);
}
// box
{
int id = numRigidBodies++;
PfxBox box(1.0f,1.0f,1.0f);
PfxShape shape;
shape.reset();
shape.setBox(box);
collidables[id].reset();
collidables[id].addShape(shape);
collidables[id].finish();
bodies[id].reset();
bodies[id].setMass(1.0f);
bodies[id].setInertia(pfxCalcInertiaBox(PfxVector3(1.0f),1.0f));
states[id].reset();
states[id].setPosition(PfxVector3(0.0f,5.0f,5.0f));
states[id].setMotionType(kPfxMotionTypeActive);
states[id].setRigidBodyId(id);
}
// capsule
{
int id = numRigidBodies++;
PfxCapsule capsule(1.5f,0.5f);
PfxShape shape;
shape.reset();
shape.setCapsule(capsule);
collidables[id].reset();
collidables[id].addShape(shape);
collidables[id].finish();
bodies[id].reset();
bodies[id].setMass(2.0f);
bodies[id].setInertia(pfxCalcInertiaCylinderX(2.0f,0.5f,2.0f));
states[id].reset();
states[id].setPosition(PfxVector3(5.0f,5.0f,0.0f));
states[id].setMotionType(kPfxMotionTypeActive);
states[id].setRigidBodyId(id);
}
// cylinder
{
int id = numRigidBodies++;
PfxCylinder cylinder(0.5f,1.5f);
PfxShape shape;
shape.reset();
shape.setCylinder(cylinder);
collidables[id].reset();
collidables[id].addShape(shape);
collidables[id].finish();
bodies[id].reset();
bodies[id].setMass(3.0f);
bodies[id].setInertia(pfxCalcInertiaCylinderX(0.5f,1.5f,3.0f));
states[id].reset();
states[id].setPosition(PfxVector3(0.0f,10.0f,0.0f));
states[id].setMotionType(kPfxMotionTypeActive);
states[id].setRigidBodyId(id);
}
// convex mesh
{
PfxCreateConvexMeshParam param;
param.verts = BarrelVtx;
param.numVerts = BarrelVtxCount;
param.vertexStrideBytes = sizeof(float)*6;
param.triangles = BarrelIdx;
param.numTriangles = BarrelIdxCount/3;
param.triangleStrideBytes = sizeof(unsigned short)*3;
PfxInt32 ret = pfxCreateConvexMesh(gConvex,param);
if(ret != SCE_PFX_OK) {
SCE_PFX_PRINTF("Can't create gConvex mesh.\n");
}
int id = numRigidBodies++;
PfxShape shape;
shape.reset();
shape.setConvexMesh(&gConvex);
collidables[id].reset();
collidables[id].addShape(shape);
collidables[id].finish();
bodies[id].reset();
bodies[id].setMass(3.0f);
bodies[id].setInertia(pfxCalcInertiaSphere(1.0f,1.0f));
states[id].reset();
states[id].setPosition(PfxVector3(0.0f,15.0f,0.0f));
states[id].setMotionType(kPfxMotionTypeActive);
states[id].setRigidBodyId(id);
}
// combined primitives
{
int id = numRigidBodies++;
//E Both shapes and incides buffer have to be kept when creating a combined shape.
static PfxShape shapes[3];
PfxUInt16 shapeIds[3]={0,1,2};
collidables[id].reset(shapes,shapeIds,3);
{
PfxBox box(0.5f,0.5f,1.5f);
PfxShape shape;
shape.reset();
shape.setBox(box);
shape.setOffsetPosition(PfxVector3(-2.0f,0.0f,0.0f));
collidables[id].addShape(shape);
}
{
PfxBox box(0.5f,1.5f,0.5f);
PfxShape shape;
shape.reset();
shape.setBox(box);
shape.setOffsetPosition(PfxVector3(2.0f,0.0f,0.0f));
collidables[id].addShape(shape);
}
{
PfxCapsule cap(1.5f,0.5f);
PfxShape shape;
shape.reset();
shape.setCapsule(cap);
collidables[id].addShape(shape);
}
collidables[id].finish();
bodies[id].reset();
bodies[id].setMass(3.0f);
bodies[id].setInertia(pfxCalcInertiaBox(PfxVector3(2.5f,1.0f,1.0f),3.0f));
states[id].reset();
states[id].setPosition(PfxVector3(0.0f,5.0f,0.0f));
states[id].setMotionType(kPfxMotionTypeActive);
states[id].setRigidBodyId(id);
}
}
int
main(int argc, char *argv[]) {
GLUquadric *quad;
GLfloat zero[] = { 0.f, 0.f, 0.f, 0.f };
glutInit(&argc, argv);
glutInitWindowSize(winWidth, winHeight);
glutInitDisplayMode(GLUT_RGBA|GLUT_DEPTH|GLUT_DOUBLE);
(void)glutCreateWindow("hatch");
#ifdef GL_EXT_blend_minmax
hasBlendMinmax = glutExtensionSupported("GL_EXT_blend_minmax");
# ifdef _WIN32
/* grab extension function pointer for glBlendEquationEXT */
if (hasBlendMinmax) {
glBlendEquationEXT = (PFNGLBLENDEQUATIONEXTPROC) wglGetProcAddress("glBlendEquationEXT");
if (glBlendEquationEXT == NULL) {
hasBlendMinmax = 0;
}
}
# endif
#endif
glutDisplayFunc(redraw);
glutReshapeFunc(reshape);
glutMouseFunc(mouse);
glutMotionFunc(motion);
glutKeyboardFunc(key);
create_menu();
/* draw a perspective scene */
glMatrixMode(GL_PROJECTION);
glFrustum(-100., 100., -100., 100., 300., 600.);
glMatrixMode(GL_MODELVIEW);
/* look at scene from (0, 0, 450) */
gluLookAt(0., 0., 450., 0., 0., 0., 0., 1., 0.);
glEnable(GL_DEPTH_TEST);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
glEnable(GL_COLOR_MATERIAL);
glColorMaterial(GL_FRONT_AND_BACK, GL_DIFFUSE);
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, zero);
glCullFace(GL_BACK);
glReadBuffer(GL_BACK);
glDisable(GL_DITHER);
glNewList(1, GL_COMPILE);
glutSolidCube(50.f);
glEndList();
glNewList(2, GL_COMPILE);
glutSolidTeapot(50.f);
glEndList();
quad = gluNewQuadric();
gluQuadricTexture(quad, GL_TRUE);
glNewList(3, GL_COMPILE);
gluSphere(quad, 70., 20, 20);
glEndList();
gluDeleteQuadric(quad);
glNewList(4, GL_COMPILE);
#if 0
glutSolidTorus(20, 50, 50, 50);
#endif
torus(20, 50, 50, 100);
glEndList();
glNewList(5, GL_COMPILE);
glPushMatrix();
glTranslatef(0.f, -80.f, 0.f);
cylinder(40, 3, 20, 160);
glTranslatef(0.f, 20.f, 0.f);
cylinder(40, 3, 60, 20);
glTranslatef(0.f, 20.f, 0.f);
cylinder(40, 3, 30, 20);
glTranslatef(0.f, 60.f, 0.f);
cylinder(40, 3, 30, 20);
glTranslatef(0.f, 20.f, 0.f);
cylinder(40, 3, 60, 20);
glPopMatrix();
glEndList();
maxobject = 5;
make_stripes();
make_stipple();
glClearColor(0.25f, 0.25f, 0.25f, 1.0f);
glClearColor(1.f, 1.f, 1.f, 1.0f);
glDepthFunc(GL_LEQUAL);
CHECK_ERROR("end of main");
help();
glutMainLoop();
return 0;
}
void DrawableBBox::draw() const
{
if (visible) {
GLfloat red[3] = {1,0,0};
double edgesDim = scaleFactor * BB_DIM;
// Bounding Box Edges
cylinder(box.getMin(), Pointd(box.getMaxX(),box.getMinY(),box.getMinZ()), edgesDim, edgesDim, red);
cylinder(box.getMin(), Pointd(box.getMinX(),box.getMinY(),box.getMaxZ()), edgesDim, edgesDim, red);
cylinder(box.getMin(), Pointd(box.getMinX(),box.getMaxY(),box.getMinZ()), edgesDim, edgesDim, red);
cylinder(box.getMax(), Pointd(box.getMinX(),box.getMaxY(),box.getMaxZ()), edgesDim, edgesDim, red);
cylinder(box.getMax(), Pointd(box.getMaxX(),box.getMaxY(),box.getMinZ()), edgesDim, edgesDim, red);
cylinder(box.getMax(), Pointd(box.getMaxX(),box.getMinY(),box.getMaxZ()), edgesDim, edgesDim, red);
cylinder(Pointd(box.getMaxX(), box.getMinY(), box.getMinZ()), Pointd(box.getMaxX(),box.getMinY(),box.getMaxZ()), edgesDim, edgesDim, red);
cylinder(Pointd(box.getMaxX(), box.getMinY(), box.getMinZ()), Pointd(box.getMaxX(),box.getMaxY(),box.getMinZ()), edgesDim, edgesDim, red);
cylinder(Pointd(box.getMinX(), box.getMinY(), box.getMaxZ()), Pointd(box.getMaxX(),box.getMinY(),box.getMaxZ()), edgesDim, edgesDim, red);
cylinder(Pointd(box.getMinX(), box.getMaxY(), box.getMaxZ()), Pointd(box.getMinX(),box.getMaxY(),box.getMinZ()), edgesDim, edgesDim, red);
cylinder(Pointd(box.getMinX(), box.getMaxY(), box.getMaxZ()), Pointd(box.getMinX(),box.getMinY(),box.getMaxZ()), edgesDim, edgesDim, red);
cylinder(Pointd(box.getMinX(), box.getMaxY(), box.getMinZ()), Pointd(box.getMaxX(),box.getMaxY(),box.getMinZ()), edgesDim, edgesDim, red);
}
}
bool UnitCylinder::intersect( Ray3D& ray, const Matrix4x4& worldToModel,
const Matrix4x4& modelToWorld ) {
Ray3D r;
r.origin = worldToModel * ray.origin;
r.dir = worldToModel * ray.dir;
Point3D cylinder(0,0,0);
double int0, int1, int2, int3, t, t1;
double a = r.dir[0] * r.dir[0] + r.dir[1] * r.dir[1];
double b = r.origin[0] * r.dir[0] + r.origin[1] * r.dir[1];
double c = r.origin[0] * r.origin[0] + r.origin[1] * r.origin[1] - 1;
double d = b * b - a * c;
if (d < 0) return false;
int0 = (-b + sqrt(d)) / a;
int1 = (-b - sqrt(d)) / a;
if (int0 < 0 && int1 < 0) return false;
else if (int0 > 0 && int1 < 0) t = int0;
else t = int1;
int2 = ( -0.5 - r.origin[2])/r.dir[2];
int3 = ( 0.5 - r.origin[2])/r.dir[2];
t1 = int2 < int3 ? int2 : int3;
if (t1 * t1 < 0.001) return false;
if (t * t < 0.001) return false;
Point3D n0(0,0,1);
Point3D n1(0,0,-1);
Vector3D tn0;
if (int2 < int3) tn0 = n1 - cylinder;
else tn0 = n0 - cylinder;
tn0.normalize();
Point3D pint = r.origin + t1 * r.dir;
if (pint[0]* pint[0] + pint[1] * pint[1] <= 1) {
if (ray.intersection.none || ray.intersection.t_value > int3) {
ray.intersection.t_value = t1;
ray.intersection.point = pint;
ray.intersection.none = false;
ray.intersection.normal = tn0;
return true;
}
}
pint = r.origin + t * r.dir;
Vector3D tn1(pint[0], pint[1], 0);
tn1.normalize();
if (pint[2] < 0.5 && pint[2] > -0.5) {
if (ray.intersection.none || ray.intersection.t_value > int3) {
ray.intersection.t_value = t;
ray.intersection.point = modelToWorld * pint;
ray.intersection.none = false;
ray.intersection.normal = modelToWorld * tn1;
return true;
}
}
return false;
}
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> Iint(512,512,0) ;
vpImage<unsigned char> Iext(512,512,0) ;
// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX displayInt;
vpDisplayX displayExt;
#elif defined VISP_HAVE_GTK
vpDisplayGTK displayInt;
vpDisplayGTK displayExt;
#elif defined VISP_HAVE_GDI
vpDisplayGDI displayInt;
vpDisplayGDI displayExt;
#endif
if (opt_display) {
try{
// Display size is automatically defined by the image (Iint) and (Iext) size
displayInt.init(Iint, 100, 100,"Internal view") ;
displayExt.init(Iext, (int)(130+Iint.getWidth()), 100, "External 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(Iint) ;
vpDisplay::display(Iext) ;
vpDisplay::flush(Iint) ;
vpDisplay::flush(Iext) ;
}
catch(...)
{
vpERROR_TRACE("Error while displaying the image") ;
exit(-1);
}
}
vpProjectionDisplay externalview ;
//Set the camera parameters
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,2,
vpMath::rad(5), vpMath::rad(5), vpMath::rad(20));
vpHomogeneousMatrix wMc, wMo;
robot.getPosition(wMc) ;
wMo = wMc * cMo; // Compute the position of the object in the world frame
// sets the final camera location (for simulation purpose)
vpHomogeneousMatrix cMod(0,0,1,
vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
// sets the cylinder coordinates in the world frame
vpCylinder cylinder(0,1,0, // direction
0,0,0, // point of the axis
0.1) ; // radius
externalview.insert(cylinder) ;
// sets the desired position of the visual feature
cylinder.track(cMod) ;
cylinder.print() ;
//Build the desired line features thanks to the cylinder and especially its paramaters in the image frame
vpFeatureLine ld[2] ;
int i ;
for(i=0 ; i < 2 ; i++)
vpFeatureBuilder::create(ld[i],cylinder,i) ;
// computes the cylinder coordinates in the camera frame and its 2D coordinates
// sets the current position of the visual feature
cylinder.track(cMo) ;
cylinder.print() ;
//Build the current line features thanks to the cylinder and especially its paramaters in the image frame
vpFeatureLine l[2] ;
for(i=0 ; i < 2 ; i++)
{
vpFeatureBuilder::create(l[i],cylinder,i) ;
l[i].print() ;
}
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA) ;
task.setInteractionMatrixType(vpServo::DESIRED,vpServo::PSEUDO_INVERSE) ;
// it can also be interesting to test these possibilities
// task.setInteractionMatrixType(vpServo::CURRENT,vpServo::PSEUDO_INVERSE) ;
// task.setInteractionMatrixType(vpServo::MEAN, vpServo::PSEUDO_INVERSE) ;
// task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) ;
// task.setInteractionMatrixType(vpServo::DESIRED, vpServo::TRANSPOSE) ;
// task.setInteractionMatrixType(vpServo::CURRENT, vpServo::TRANSPOSE) ;
// we want to see 2 lines on 2 lines
task.addFeature(l[0],ld[0]) ;
task.addFeature(l[1],ld[1]) ;
// Set the point of view of the external view
vpHomogeneousMatrix cextMo(0,0,6,
vpMath::rad(40), vpMath::rad(10), vpMath::rad(60)) ;
// Display the initial scene
vpServoDisplay::display(task,cam,Iint) ;
externalview.display(Iext,cextMo, cMo, cam, vpColor::red);
vpDisplay::flush(Iint) ;
vpDisplay::flush(Iext) ;
// Display task information
task.print() ;
if (opt_display && opt_click_allowed) {
std::cout << "\n\nClick in the internal camera view window to start..." << std::endl;
vpDisplay::getClick(Iint) ;
}
// set the gain
task.setLambda(1) ;
// Display task information
task.print() ;
unsigned int iter=0 ;
// The first loop is needed to reach the desired position
do
{
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
// retrieve x,y and Z of the vpLine structure
// Compute the parameters of the cylinder in the camera frame and in the image frame
cylinder.track(cMo) ;
//Build the current line features thanks to the cylinder and especially its paramaters in the image frame
for(i=0 ; i < 2 ; i++)
{
vpFeatureBuilder::create(l[i],cylinder,i) ;
}
// Display the current scene
if (opt_display) {
vpDisplay::display(Iint) ;
vpDisplay::display(Iext) ;
vpServoDisplay::display(task,cam,Iint) ;
externalview.display(Iext,cextMo, cMo, cam, vpColor::red);
vpDisplay::flush(Iint);
vpDisplay::flush(Iext);
}
// 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 ;
}
while(( task.getError() ).sumSquare() > 1e-9) ;
// Second loop is to compute the control law while taking into account the secondary task.
// In this example the secondary task is cut in four steps.
// The first one consists in impose a movement of the robot along the x axis of the object frame with a velocity of 0.5.
// The second one consists in impose a movement of the robot along the y axis of the object frame with a velocity of 0.5.
// The third one consists in impose a movement of the robot along the x axis of the object frame with a velocity of -0.5.
// The last one consists in impose a movement of the robot along the y axis of the object frame with a velocity of -0.5.
// Each steps is made during 200 iterations.
vpColVector e1(6) ; e1 = 0 ;
vpColVector e2(6) ; e2 = 0 ;
vpColVector proj_e1 ;
vpColVector proj_e2 ;
iter = 0;
double rapport = 0;
double vitesse = 0.5;
unsigned int tempo = 800;
while(iter < tempo)
{
vpColVector v ;
robot.getPosition(wMc) ;
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
cylinder.track(cMo) ;
for(i=0 ; i < 2 ; i++)
{
vpFeatureBuilder::create(l[i],cylinder,i) ;
}
if (opt_display)
{
vpDisplay::display(Iint) ;
vpDisplay::display(Iext) ;
vpServoDisplay::display(task,cam,Iint) ;
externalview.display(Iext,cextMo, cMo, cam, vpColor::red);
vpDisplay::flush(Iint);
vpDisplay::flush(Iext);
}
v = task.computeControlLaw() ;
if ( iter%tempo < 200 /*&& iter%tempo >= 0*/)
{
e2 = 0;
e1[0] = fabs(vitesse) ;
proj_e1 = task.secondaryTask(e1);
rapport = vitesse/proj_e1[0];
proj_e1 *= rapport ;
v += proj_e1 ;
}
if ( iter%tempo < 400 && iter%tempo >= 200)
{
e1 = 0;
e2[1] = fabs(vitesse) ;
proj_e2 = task.secondaryTask(e2);
rapport = vitesse/proj_e2[1];
proj_e2 *= rapport ;
v += proj_e2 ;
}
if ( iter%tempo < 600 && iter%tempo >= 400)
{
e2 = 0;
e1[0] = -fabs(vitesse) ;
proj_e1 = task.secondaryTask(e1);
rapport = -vitesse/proj_e1[0];
proj_e1 *= rapport ;
v += proj_e1 ;
}
if ( iter%tempo < 800 && iter%tempo >= 600)
{
e1 = 0;
e2[1] = -fabs(vitesse) ;
proj_e2 = task.secondaryTask(e2);
rapport = -vitesse/proj_e2[1];
proj_e2 *= rapport ;
v += proj_e2 ;
}
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ;
iter++;
}
if (opt_display && opt_click_allowed) {
std::cout << "\nClick in the internal camera view window to end..." << std::endl;
vpDisplay::getClick(Iint) ;
}
// Display task information
task.print() ;
task.kill();
}
