void SLi_gen::SetSlicePos(const P3& lp0, const P3& lp1)
{
p0 = lp0;
p1 = lp1;
P3 v01 = p1 - p0;
v01n = v01 / v01.Len();
p0p = p0 - v01n * Dot(v01n, p0);
TOL_ZERO(Dot(p0p, v01n));
// find two perpendicular axes
perp1 = P3::CrossProd(P3(0,0,1), v01n);
if (perp1.Lensq() < 0.001)
perp1 = P3::CrossProd((fabs(v01n.x) < fabs(v01n.y) ? P3(1,0,0) : P3(0,1,0)), v01n);
ASSERT(perp1.Lensq() != 0);
perp2 = P3::CrossProd(perp1, v01n);
TOL_ZERO(Dot(v01n, perp1));
TOL_ZERO(Dot(v01n, perp2));
axis = P2(Dot(perp1, p0), Dot(perp2, p1));
// the array of intersections
inter.clear();
};
/*!
Build a vpMbtDistanceLine thanks to two points corresponding to the extremities.
\param _p1 : The first extremity.
\param _p2 : The second extremity.
*/
void
vpMbtDistanceLine::buildFrom(vpPoint &_p1, vpPoint &_p2)
{
if (line == NULL) {
line = new vpLine ;
}
poly.setNbPoint(2);
poly.addPoint(0, _p1);
poly.addPoint(1, _p2);
p1 = &poly.p[0];
p2 = &poly.p[1];
vpColVector V1(3);
vpColVector V2(3);
vpColVector V3(3);
vpColVector V4(3);
V1[0] = p1->get_oX();
V1[1] = p1->get_oY();
V1[2] = p1->get_oZ();
V2[0] = p2->get_oX();
V2[1] = p2->get_oY();
V2[2] = p2->get_oZ();
//if((V1-V2).sumSquare()!=0)
if(std::fabs((V1-V2).sumSquare()) > std::numeric_limits<double>::epsilon())
{
{
V3[0]=double(rand()%1000)/100;
V3[1]=double(rand()%1000)/100;
V3[2]=double(rand()%1000)/100;
vpColVector v_tmp1,v_tmp2;
v_tmp1 = V2-V1;
v_tmp2 = V3-V1;
V4=vpColVector::cross(v_tmp1,v_tmp2);
}
vpPoint P3(V3[0],V3[1],V3[2]);
vpPoint P4(V4[0],V4[1],V4[2]);
buildLine(*p1,*p2, P3,P4, *line) ;
}
else
{
vpPoint P3(V1[0],V1[1],V1[2]);
vpPoint P4(V2[0],V2[1],V2[2]);
buildLine(*p1,*p2,P3,P4,*line) ;
}
}
Slots(const P1& plugin1 = P1(),
const P2& plugin2 = P2(),
const P3& plugin3 = P3(),
const P4& plugin4 = P4(),
const P5& plugin5 = P5())
: BaseT( plugin1, BaseSlots(plugin2, plugin3, plugin4, plugin5, NullT()) )
{ }
/**
* Returns the probability of the cell having dynamics after 2^N
* Observations. The larger the N, and higher the value, the more "Semi-static"
* is the cell. Note that the filter returns
* 0.5 >= P <=1.0 and P = 0.0 only if there is not enough evidence about the
* Behaviour
*/
float computeSemiStaticLikelihood(int N)
{
if( (b_entry_event + b_exit_event)<20.0 )
{
return 0.0f;
}
Eigen::Matrix2f P;
Eigen::Vector2f u2(0, 1.0);
Eigen::Vector2f u3(1.0, 0);
float Lex = exitL();
float Len = entryL();
Eigen::Vector2f P2,P3;
P(0,0) = (1.0-Len);
P(0,1) = Len;
P(1,0) = Lex;
P(1,1) = (1-Lex);
for(int i=0; i<N; i++) P = P*P;
P2 = u2.transpose() * P;
P3 = u3.transpose() * P;
float Po = P2(1);
float Pu = binaryBayesUpdate(Po, P3(0));
normalizeProb(Pu);
return Pu;
}
void test_polygon_quad_subdivide()
{
Point_E3d P0(-20,-0.6,-20);
Point_E3d P1( 20,-0.6,-20);
Point_E3d P2( 20,-0.6, 20);
Point_E3d P3(-20,-0.6, 20);
const Point_E3d points[] = { P0, P1, P2, P3 };
{
Polygon_E3d P(points, points+4);
Array2<Point_E3d> A = quad_subdivide(P, 4, 4);
assert(A(0,0) == P0);
assert(A(4,0) == P1);
assert(A(4,4) == P2);
assert(A(0,4) == P3);
assert(A(2,1) == Point_E3d( 0,-0.6,-10));
assert(A(2,2) == Point_E3d( 0,-0.6, 0));
assert(A(1,2) == Point_E3d(-10,-0.6, 0));
}
{
Polygon_E3d P(points, points+4);
Array2<Point_E3d> A = quad_subdivide(P, 4, 2);
assert(A(0,0) == P0);
assert(A(4,0) == P1);
assert(A(4,2) == P2);
assert(A(0,2) == P3);
assert(A(2,1) == Point_E3d( 0,-0.6, 0));
assert(A(2,2) == Point_E3d( 0,-0.6, 20));
assert(A(1,2) == Point_E3d(-10,-0.6, 20));
}
}
static void computeLevelset(GModel *gm, cartesianBox<double> &box)
{
// tolerance for desambiguation
const double tol = box.getLC() * 1.e-12;
std::vector<SPoint3> nodes;
std::vector<int> indices;
for (cartesianBox<double>::valIter it = box.nodalValuesBegin();
it != box.nodalValuesEnd(); ++it){
nodes.push_back(box.getNodeCoordinates(it->first));
indices.push_back(it->first);
}
Msg::Info(" %d nodes in the grid at level %d", (int)nodes.size(), box.getLevel());
std::vector<double> dist, localdist;
std::vector<SPoint3> dummy;
for (GModel::fiter fit = gm->firstFace(); fit != gm->lastFace(); fit++){
for (int i = 0; i < (*fit)->stl_triangles.size(); i += 3){
int i1 = (*fit)->stl_triangles[i];
int i2 = (*fit)->stl_triangles[i + 1];
int i3 = (*fit)->stl_triangles[i + 2];
GPoint p1 = (*fit)->point((*fit)->stl_vertices[i1]);
GPoint p2 = (*fit)->point((*fit)->stl_vertices[i2]);
GPoint p3 = (*fit)->point((*fit)->stl_vertices[i3]);
SPoint2 p = ((*fit)->stl_vertices[i1] + (*fit)->stl_vertices[i2] +
(*fit)->stl_vertices[i3]) * 0.33333333;
SVector3 N = (*fit)->normal(p);
SPoint3 P1(p1.x(), p1.y(), p1.z());
SPoint3 P2(p2.x(), p2.y(), p2.z());
SPoint3 P3(p3.x(), p3.y(), p3.z());
SVector3 NN(crossprod(P2 - P1, P3 - P1));
if (dot(NN, N) > 0)
signedDistancesPointsTriangle(localdist, dummy, nodes, P1, P2, P3);
else
signedDistancesPointsTriangle(localdist, dummy, nodes, P2, P1, P3);
if(dist.empty())
dist = localdist;
else{
for (unsigned int j = 0; j < localdist.size(); j++){
// FIXME: if there is an ambiguity assume we are inside (to
// avoid holes in the structure). This is definitely just a
// hack, as it could create pockets of matter outside the
// structure...
if(dist[j] * localdist[j] < 0 &&
fabs(fabs(dist[j]) - fabs(localdist[j])) < tol){
dist[j] = std::max(dist[j], localdist[j]);
}
else{
dist[j] = (fabs(dist[j]) < fabs(localdist[j])) ? dist[j] : localdist[j];
}
}
}
}
}
for (unsigned int j = 0; j < dist.size(); j++)
box.setNodalValue(indices[j], dist[j]);
if(box.getChildBox()) computeLevelset(gm, *box.getChildBox());
}
void main() {
Polygon P1, P2(6, 4), P3(10, 4, 5.6, 738);
std::cout << "The Perimeter of the polygon is " << P1.getPerimeter() << std::endl <<
"The area of the polygon is " << P1.getArea() << std::endl;
std::cout << "The Perimeter of the polygon is " << P2.getPerimeter() << std::endl <<
"The area of the polygon is " << P2.getArea() << std::endl;
std::cout << "The Perimeter of the polygon is " << P3.getPerimeter() << std::endl <<
"The area of the polygon is " << P3.getArea() << std::endl;
}
AREXPORT ArMapObject::ArMapObject(const char *type,
ArPose pose,
const char *description,
const char *iconName,
const char *name,
bool hasFromTo,
ArPose fromPose,
ArPose toPose) :
myType((type != NULL) ? type : ""),
myBaseType(),
myName((name != NULL) ? name : "" ),
myDescription((description != NULL) ? description : "" ),
myPose(pose),
myIconName((iconName != NULL) ? iconName : "" ),
myHasFromTo(hasFromTo),
myFromPose(fromPose),
myToPose(toPose),
myFromToSegments(),
myStringRepresentation()
{
if (myHasFromTo)
{
double angle = myPose.getTh();
double sa = ArMath::sin(angle);
double ca = ArMath::cos(angle);
double fx = fromPose.getX();
double fy = fromPose.getY();
double tx = toPose.getX();
double ty = toPose.getY();
ArPose P0((fx*ca - fy*sa), (fx*sa + fy*ca));
ArPose P1((tx*ca - fy*sa), (tx*sa + fy*ca));
ArPose P2((tx*ca - ty*sa), (tx*sa + ty*ca));
ArPose P3((fx*ca - ty*sa), (fx*sa + ty*ca));
myFromToSegments.push_back(ArLineSegment(P0, P1));
myFromToSegments.push_back(ArLineSegment(P1, P2));
myFromToSegments.push_back(ArLineSegment(P2, P3));
myFromToSegments.push_back(ArLineSegment(P3, P0));
myFromToSegment.newEndPoints(fromPose, toPose);
}
else { // pose
size_t whPos = myType.rfind("WithHeading");
size_t whLen = 11;
if (whPos > 0) {
if (whPos == myType.size() - whLen) {
myBaseType = myType.substr(0, whPos);
}
}
} // end else pose
IFDEBUG(
ArLog::log(ArLog::Normal,
"ArMapObject::ctor() created %s (%s)",
myName.c_str(), myType.c_str());
);
int main() {
__CPROVER_ASYNC_0: P0(0);
__CPROVER_ASYNC_1: P1(0);
__CPROVER_ASYNC_2: P2(0);
__CPROVER_ASYNC_3: P3(0);
__CPROVER_assume(__unbuffered_cnt==4);
fence();
// EXPECT:exists
__CPROVER_assert(!(x==2 && z==2 && __unbuffered_p1_EAX==0 && __unbuffered_p3_EAX==2 && __unbuffered_p3_EBX==0), "Program proven to be relaxed for X86, model checker says YES.");
return 0;
}
void MainWindow::resizeItem() {
for (int i = 0; i < 10; i++) {
QGraphicsRectItem* theFrame = item1;
QPointF P1(100 - (7.0 * i)/3.0, 50 -(5.0*i)/3.0);
graphicsSheet->addPoint("P1", P1);
graphicsSheet->addPoint("pos", theFrame->pos());
QPointF P2i = QPointF(theFrame->rect().width(), theFrame->rect().height());
QPointF P2 = theFrame->mapToScene(P2i);
qDebug() << "P1:" << P1;
qDebug() << "P2:" << P2;
graphicsSheet->addPoint("P2", P2);
QTransform t;
t.rotate(-theFrame->rotation());
t.translate(-P1.x(), -P1.y());
QPointF P2t = t.map(P2);
// qDebug() << "P2t:" << P2t; // OK
/*************************/
QPointF P3(P2t.x()/2, P2t.y()/2);
// qDebug() << "P3:" << P3;
QTransform t2;
t2.translate(P1.x(), P1.y());
t2.rotate(theFrame->rotation());
QPointF P3t = t2.map(P3);
//// qDebug() << "P3t:" << P3t; // OK
graphicsSheet->addPoint("P3t", P3t);
QTransform t3;
t3.translate(P3t.x(), P3t.y());
t3.rotate(-theFrame->rotation());
t3.translate(-(P3t.x()), -(P3t.y()));
QPointF newPos = t3.map(P1);
QSizeF newSize = QSizeF(P2t.x(), P2t.y());
graphicsSheet->addPoint("newPos", newPos);
RectItem* item1 = new RectItem(QRectF(newPos.x(), newPos.y(), newSize.width(), newSize.height()));
qreal angle = 30;
QPointF center2 = QPointF(item1->rect().width() / 2, item1->rect().height() / 2);
item1->setTransformOriginPoint(center2);
item1->setRotation(angle);
graphicsSheet->scene()->addItem(item1);
}
}
int main() {
__CPROVER_ASYNC_0: P0(0);
__CPROVER_ASYNC_1: P1(0);
__CPROVER_ASYNC_2: P2(0);
__CPROVER_ASYNC_3: P3(0);
__CPROVER_assume(__unbuffered_cnt==4);
fence();
// EXPECT:exists
__CPROVER_assert(!(x==2 && y==2 && __unbuffered_p0_r1==2 && __unbuffered_p2_r1==2), "Program proven to be relaxed for PPC, model checker says YES.");
return 0;
}
Word ADJ_2D(Word c, Word c_l, Word c_r, Word P, Word J)
{
Word U,V,W,v_l,Sol;
v_l = LDCOEFMASK(c,P,J);
U = AD2DS_CONS(c_l,P);
V = AD2DS_CONS(c,P);
W = AD2DS_CONS(c_r,P);
Sol = P3(U,V,W,v_l,FIRST(LELTI(c,INDX)));
return Sol;
}
void CameraScreen::reinit() {
Camera::reinit();
// find the position of the center of the screen in 3D space
this->centerScreen = position + (screenDist * direction);
double cosrot = cos(rotation);
double sinrot = sin(rotation);
// compute the directions of the u and v vector along the screen
P3S sDir(direction);
P3S sdu(P3(cosrot,0.0,sinrot));
P3S sdv(P3(sinrot,0.0,cosrot));
sdu.u += sDir.u + M_PI/2;
sdv.v += sDir.v;
P3 du(sdu);
P3 dv(sdv);
du.normalize();
dv.normalize();
// deduce dx and dy
this->dx = du;
this->dy = - dv;
// precompute the top left corner of the screen
int width = screen->getWidth();
int height = screen->getHeight();
this->topLeftCornerScreen = centerScreen - ((width/2)*dx) - ((height/2)*dy);
}
Point Rectangle::Sample(float u, float v, Normal *Ns) const {
// u and v are random samples on the surface of the light source
Point P0(-x/2, y/2, height), P1(x/2, y/2, height);
Point P2(-x/2, -y/2, height), P3(x/2, -y/2, height);
Point p = P0 + (P1 - P0) * u + (P2 - P0) * v;
Normal n = Normal(Cross(P2-P0, P1-P0));
*Ns = Normalize(n);
// NORMAL ON THE PLANE
*Ns = Normalize((*ObjectToWorld)(n));
if (ReverseOrientation) *Ns *= -1.f;
return (*ObjectToWorld)(p);
}
QPolygon KDChart::TextLayoutItem::rotatedCorners() const
{
// the angle in rad
const qreal angle = mAttributes.rotation() * PI / 180.0;
QSize size = unrotatedSizeHint();
// my P1 - P4 (the four points of the rotated area)
QPointF P1( size.height() * sin( angle ), 0 );
QPointF P2( size.height() * sin( angle ) + size.width() * cos( angle ), size.width() * sin( angle ) );
QPointF P3( size.width() * cos( angle ), size.width() * sin( angle ) + size.height() * cos( angle ) );
QPointF P4( 0, size.height() * cos( angle ) );
QPolygon result;
result << P1.toPoint() << P2.toPoint() << P3.toPoint() << P4.toPoint();
return result;
}
bool Triangle::intersect(Ray & ray, double * distFromSource) {
P3 R0 = ray.getSource();
P3 Rd = ray.getDirection();
P3 distV = P3(R0, p);
P3 N = normN * normal;
double nr = N * Rd;
if(fabs(nr) < 0.001) return false; // colineaire
double ir = (N * distV) / nr;
*distFromSource = ir;
if (ir < 0.001) return false;
double iu = -((distV ^ AC) * Rd) / nr;
double iv = -((AB ^ distV) * Rd) / nr;
*distFromSource = ir;
return (ir >= 0 && iu >= 0 && iv >= 0 && iu + iv <= 1);
}
void srTSpherMirror::SetupInAndOutPlanes(TVector3d* InPlane, TVector3d* OutPlane)
{// Assumes InPlaneNorm and transverse part InPlaneCenPo already defined !!!
TVector3d &InPlaneCenPo = *InPlane, &InPlaneNorm = InPlane[1];
TVector3d &OutPlaneCenPo = *OutPlane, &OutPlaneNorm = OutPlane[1];
TVector3d LocInPlaneNorm = InPlaneNorm;
FromLabToLocFrame_Vector(LocInPlaneNorm);
double xP = -0.5*Dx, yP = -0.5*Dy;
TVector3d P0(xP, yP, SurfaceFunction(xP, yP, 0));
TVector3d P1(-xP, yP, SurfaceFunction(-xP, yP, 0));
TVector3d P2(xP, -yP, SurfaceFunction(xP, -yP, 0));
TVector3d P3(-xP, -yP, SurfaceFunction(-xP, -yP, 0));
TVector3d EdgePoints[] = { P0, P1, P2, P3 };
int LowestInd, UppestInd;
FindLowestAndUppestPoints(LocInPlaneNorm, EdgePoints, 4, LowestInd, UppestInd);
TVector3d PointForInPlane = EdgePoints[LowestInd];
FromLocToLabFrame_Point(PointForInPlane);
InPlaneCenPo.y = PointForInPlane.y;
TVector3d LocInPlaneCenPo = InPlaneCenPo;
FromLabToLocFrame_Point(LocInPlaneCenPo);
TVector3d LocInPlane[] = { LocInPlaneCenPo, LocInPlaneNorm };
TVector3d LocP, LocN;
FindRayIntersectWithSurface(LocInPlane, 0, LocP);
SurfaceNormalAtPoint(LocP.x, LocP.y, 0, LocN);
TVector3d LocOutPlaneNorm = LocInPlaneNorm;
ReflectVect(LocN, LocOutPlaneNorm);
FindLowestAndUppestPoints(LocOutPlaneNorm, EdgePoints, 4, LowestInd, UppestInd);
OutPlaneCenPo = (EdgePoints[UppestInd]*LocOutPlaneNorm)*LocOutPlaneNorm;
*OutPlaneInLocFrame = OutPlaneCenPo; OutPlaneInLocFrame[1] = LocOutPlaneNorm;
FromLocToLabFrame_Point(OutPlaneCenPo);
OutPlaneNorm = LocOutPlaneNorm;
FromLocToLabFrame_Vector(OutPlaneNorm);
TVector3d LabN = LocN;
FromLocToLabFrame_Vector(LabN);
ExRefInLabFrameBeforeProp = TVector3d(1.,0.,0.);
ReflectVect(LabN, ExRefInLabFrameBeforeProp);
EzRefInLabFrameBeforeProp = TVector3d(0.,0.,1.);
ReflectVect(LabN, EzRefInLabFrameBeforeProp);
}
int main(){
int a,b;
Point P1;
scanf("%d%d",&a,&b);
Point P2(a,b);
scanf("%d%d",&a,&b);
Point P3(a,b);
if (P1.IsOrigin()) printf("P1 origin\n");
else printf("P1 bukan origin\n");
if (P2.IsOrigin()) printf("P2 origin\n");
else printf("P2 bukan origin\n");
if (P3.IsOrigin()) printf("P3 origin\n");
else printf("P3 bukan origin\n");
Point P4 = P1.Add(P2,P3);
Point P5 = P2.Add(P3);
if (P2.IsEqual(P5)) printf("P2 equal P5\n");
else printf("P2 not equal P5\n");
if (P4.IsEqual(P5)) printf("P4 equal P5\n");
else printf("P4 not equal P5\n");
scanf("%d%d",&a,&b);
Point P6 = P2.Add(a,b);
scanf("%d%d",&a,&b);
Point P7 = P3.Add(a,b);
P4.AddToMe(P2);
scanf("%d%d",&a,&b);
P5.AddToMe(a,b);
printf("Kuadran P2 : %d\n",P2.Kuadran());
printf("Kuadran P3 : %d\n",P3.Kuadran());
printf("Kuadran P6 : %d\n",P6.Kuadran());
printf("Kuadran P7 : %d\n",P7.Kuadran());
printf("P2 : ");
PrintPoint(P2);
printf("P3 : ");
PrintPoint(P3);
printf("P4 : ");
PrintPoint(P4);
printf("P5 : ");
PrintPoint(P5);
printf("P6 : ");
PrintPoint(P6);
printf("P7 : ");
PrintPoint(P7);
return 0;
}
int main() {
Bitmap bitmap(640, 480);
Camera cam; cam.zoom = 1, cam.focus = 10000, cam.height = 100;
BezObj teapot(INPUT);
teapot.move(P3(0, -100, -1000));
teapot.rotateZ(-45);
teapot.rotateX(-45);
teapot.split(2);
cam.shot(teapot, bitmap);
ImageProcessor ip(&bitmap);
ip.rerange(
P2(-320, -240),
P2(0, 0)
);
ip.GaussianBlur(2);
bitmap.save(OUTPUT);
system(OUTPUT);
return 0;
}
AREXPORT void ArForbiddenRangeDevice::processMap(void)
{
std::list<ArMapObject *>::const_iterator it;
ArMapObject *obj;
myDataMutex.lock();
ArUtil::deleteSet(mySegments.begin(), mySegments.end());
mySegments.clear();
for (it = myMap->getMapObjects()->begin();
it != myMap->getMapObjects()->end();
it++)
{
obj = (*it);
if (strcmp(obj->getType(), "ForbiddenLine") == 0 &&
obj->hasFromTo())
{
mySegments.push_back(new ArLineSegment(obj->getFromPose(),
obj->getToPose()));
}
if (strcmp(obj->getType(), "ForbiddenArea") == 0 &&
obj->hasFromTo())
{
double angle = obj->getPose().getTh();
double sa = ArMath::sin(angle);
double ca = ArMath::cos(angle);
double fx = obj->getFromPose().getX();
double fy = obj->getFromPose().getY();
double tx = obj->getToPose().getX();
double ty = obj->getToPose().getY();
ArPose P0((fx*ca - fy*sa), (fx*sa + fy*ca));
ArPose P1((tx*ca - fy*sa), (tx*sa + fy*ca));
ArPose P2((tx*ca - ty*sa), (tx*sa + ty*ca));
ArPose P3((fx*ca - ty*sa), (fx*sa + ty*ca));
mySegments.push_back(new ArLineSegment(P0, P1));
mySegments.push_back(new ArLineSegment(P1, P2));
mySegments.push_back(new ArLineSegment(P2, P3));
mySegments.push_back(new ArLineSegment(P3, P0));
}
}
myDataMutex.unlock();
}
void ModelClass::CalculateNormal(int i1, int i2, int i3)
{
D3DXVECTOR3 P1(m_model[i1].x, m_model[i1].y, m_model[i1].z),
P2(m_model[i2].x, m_model[i2].y, m_model[i2].z),
P3(m_model[i3].x, m_model[i3].y, m_model[i3].z), Q12, Q13;
float x, y, z;
// Find two vectors on the plane of the polygan
Q12 = P2 - P1;
Q13 = P3 - P1;
// Cross product of Q12 and Q13 to find a direction
// perpendicular to the polygon
x = Q12.y*Q13.z - Q12.z*Q13.y;
y = Q12.z*Q13.x - Q12.x*Q13.z;
z = Q12.x*Q13.y - Q12.y*Q13.x;
// Normalize the normal vector...
float mag = sqrt(x*x + y*y + z*z);
x /= mag;
y /= mag;
z /= mag;
m_model[i1].nx = x;
m_model[i1].ny = y;
m_model[i1].nz = z;
m_model[i2].nx = x;
m_model[i2].ny = y;
m_model[i2].nz = z;
m_model[i3].nx = x;
m_model[i3].ny = y;
m_model[i3].nz = z;
return;
}
#include <stdlib.h>
#include <stddef.h>
#include <stdint.h>
#include <string.h>
// Set up lookup variables
#define P1(a, b, c) \
{ KJG_GENO_PACK(a, b, c, 0), \
KJG_GENO_PACK(a, b, c, 1), \
KJG_GENO_PACK(a, b, c, 2), \
KJG_GENO_PACK(a, b, c, 3) }
#define P2(a, b) { P1(a, b, 0), P1(a, b, 1), P1(a, b, 2), P1(a, b, 3) }
#define P3(a) { P2(a, 0), P2(a, 1), P2(a, 2), P2(a, 3) }
const uint8_t KJG_GENO_PACK_LOOKUP[4][4][4][4] =
{ P3(0), P3(1), P3(2), P3(3) };
#define U1(p) \
KJG_GENO_UNPACK(p), \
KJG_GENO_UNPACK(p + 1), \
KJG_GENO_UNPACK(p + 2), \
KJG_GENO_UNPACK(p + 3)
#define U2(p) U1(p), U1(p + 4), U1(p + 8), U1(p + 12)
#define U3(p) U2(p), U2(p + 16), U2(p + 32), U2(p + 48)
const uint8_t KJG_GENO_UNPACK_LOOKUP[256][4] =
{ U3(0), U3(64), U3(128),
U3(192) };
#define A1(p) \
KJG_GENO_SUM_ALT(p), \
KJG_GENO_SUM_ALT(p + 1), \
P3 Config::jsonToP3(Json::Value & p3JSON) {
return P3(p3JSON["x"].asDouble(),
p3JSON["y"].asDouble(),
p3JSON["z"].asDouble());
}
static void
irt_rpf_nominal_deriv1(const double *spec,
const double *param,
const double *where,
const double *weight, double *out)
{
int nfact = spec[RPF_ISpecDims];
int ncat = spec[RPF_ISpecOutcomes];
double aTheta = dotprod(param, where, nfact);
double aTheta2 = aTheta * aTheta;
Eigen::VectorXd num(ncat);
Eigen::VectorXd ak(ncat);
_nominal_rawprob2(spec, param, where, aTheta, ak.data(), num.data());
Eigen::VectorXd P(ncat);
Eigen::VectorXd P2(ncat);
Eigen::VectorXd P3(ncat);
Eigen::VectorXd ak2(ncat);
Eigen::VectorXd dat_num(ncat);
double numsum = 0;
double numakD = 0;
double numak2D2 = 0;
Eigen::VectorXd numakDTheta_numsum(nfact);
for (int kx=0; kx < ncat; kx++) {
ak2[kx] = ak[kx] * ak[kx];
dat_num[kx] = weight[kx]/num[kx];
numsum += num[kx];
numakD += num[kx] * ak[kx];
numak2D2 += num[kx] * ak2[kx];
}
double numsum2 = numsum * numsum;
for (int kx=0; kx < ncat; kx++) {
P[kx] = num[kx]/numsum;
P2[kx] = P[kx] * P[kx];
P3[kx] = P2[kx] * P[kx];
}
double sumNumak = dotprod(num.data(), ak.data(), ncat);
for (int fx=0; fx < nfact; fx++) {
numakDTheta_numsum[fx] = sumNumak * where[fx] / numsum;
}
for (int jx = 0; jx < nfact; jx++) {
double tmpvec = 0;
for(int i = 0; i < ncat; i++) {
tmpvec += dat_num[i] * (ak[i] * where[jx] * P[i] -
P[i] * numakDTheta_numsum[jx]) * numsum;
}
out[jx] -= tmpvec;
}
int dkoffset;
if (nfact == 0) {
dkoffset = 0;
} else {
dkoffset = ncat - 1;
}
for(int i = 1; i < ncat; i++) {
if (nfact) {
double offterm = makeOffterm(weight, P[i], aTheta, ncat, i);
double tmpvec = dat_num[i] * (aTheta * P[i] - P2[i] * aTheta) * numsum - offterm;
out[nfact + i - 1] -= tmpvec;
}
double offterm2 = makeOffterm(weight, P[i], 1, ncat, i);
double tmpvec2 = dat_num[i] * (P[i] - P2[i]) * numsum - offterm2;
out[nfact + dkoffset + i - 1] -= tmpvec2;
}
int hessbase = nfact + (ncat-1) + dkoffset;
int d2ind = 0;
//a's
for (int j = 0; j < nfact; j++) {
for (int k = 0; k <= j; k++) {
double tmpvec = 0;
for (int i = 0; i < ncat; i++) {
tmpvec += dat_num[i] * (ak2[i] * where[j] * where[k] * P[i] -
ak[i] * where[j] * P[i] * numakDTheta_numsum[k] -
ak[i] * where[k] * P[i] * numakDTheta_numsum[j] +
2 * P[i] * numakD * where[j] * numakD * where[k] / numsum2 -
P[i] * numak2D2 * where[j] * where[k] / numsum) * numsum -
dat_num[i] * (ak[i] * where[j] * P[i] - P[i] * numakDTheta_numsum[j]) *
numsum * ak[i] * where[k] +
dat_num[i] * (ak[i] * where[j] * P[i] - P[i] * numakDTheta_numsum[j]) *
numakD * where[k];
}
out[hessbase + d2ind++] -= tmpvec;
}
}
//a's with ak and d
for(int k = 1; k < ncat; k++){
int akrow = hessbase + (nfact+k)*(nfact+k-1)/2;
int dkrow = hessbase + (nfact+ncat+k-1)*(nfact+ncat+k-2)/2;
for(int j = 0; j < nfact; j++){
double tmpvec = 0;
double tmpvec2 = 0;
for(int i = 0; i < ncat; i++){
if(i == k){
tmpvec += dat_num[i] * (ak[i]*where[j] * aTheta*P[i] -
aTheta*P[i]*numakDTheta_numsum[j] +
where[j]*P[i] - 2*ak[i]*where[j]*aTheta*P2[i] +
2*aTheta*P2[i]*numakDTheta_numsum[j] -
where[j]*P2[i])*numsum -
dat_num[i]*(aTheta*P[i] - aTheta*P2[i])*numsum*ak[i]*where[j] +
dat_num[i]*(aTheta*P[i] - aTheta*P2[i])*(numakD*where[j]);
tmpvec2 += dat_num[i]*(ak[i]*where[j]*P[i] -
2*ak[i]*where[j]*P2[i] -
P[i]*numakDTheta_numsum[j] +
2*P2[i]*numakDTheta_numsum[j])*numsum -
dat_num[i]*(P[i] - P2[i])*numsum*ak[i]*where[j] +
dat_num[i]*(P[i] - P2[i])*(numakD*where[j]);
} else {
tmpvec += -weight[i]*ak[k]*aTheta*where[j]*P[k] +
weight[i]*P[k]*aTheta*numakDTheta_numsum[j] -
weight[i]*P[k]*where[j];
tmpvec2 += -weight[i]*ak[k]*where[j]*P[k] +
weight[i]*P[k]*numakDTheta_numsum[j];
}
}
out[akrow + j] -= tmpvec;
out[dkrow + j] -= tmpvec2;
}
}
//ak's and d's
for(int j = 1; j < ncat; j++){
int akrow = hessbase + (nfact+j)*(nfact+j-1)/2;
int dkrow = hessbase + (nfact+dkoffset+j)*(nfact+dkoffset+j-1)/2;
double tmpvec = makeOffterm(weight, P2[j], aTheta2, ncat, j);
double tmpvec2 = makeOffterm(weight, P[j], aTheta2, ncat, j);
double offterm = tmpvec - tmpvec2;
tmpvec = makeOffterm(weight, P2[j], 1, ncat, j);
tmpvec2 = makeOffterm(weight, P[j], 1, ncat, j);
double offterm2 = tmpvec - tmpvec2;
if (nfact) {
out[akrow + nfact + j - 1] -=
(dat_num[j]*(aTheta2*P[j] - 3*aTheta2*P2[j] +
2*aTheta2*P3[j])*numsum - weight[j]/num[j] *
(aTheta*P[j] - aTheta*P2[j])*numsum*aTheta + weight[j] *
(aTheta*P[j] - aTheta*P2[j])*aTheta + offterm);
}
out[dkrow + nfact + dkoffset + j - 1] -=
(dat_num[j]*(P[j] - 3*P2[j] + 2*P3[j])*numsum - weight[j]/num[j] *
(P[j] - P2[j])*numsum + weight[j] *
(P[j] - P2[j]) + offterm2);
for(int i = 1; i < ncat; i++) {
if(j > i) {
if (nfact) {
offterm = makeOffterm2(weight, P[j], P[i], aTheta2, ncat, i);
tmpvec = dat_num[i] * (-aTheta2*P[i]*P[j] + 2*P2[i] *aTheta2*P[j])*numsum +
dat_num[i] * (aTheta*P[i] - P2[i] * aTheta)*aTheta*num[j]+offterm;
out[akrow + nfact + i - 1] -= tmpvec;
}
offterm2 = makeOffterm2(weight, P[j], P[i], 1, ncat, i);
tmpvec2 = dat_num[i] * (-P[i]*P[j] + 2*P2[i] *P[j]) * numsum +
dat_num[i] * (P[i] - P2[i]) * num[j] + offterm2;
out[dkrow + nfact + dkoffset + i - 1] -= tmpvec2;
}
if (nfact == 0) continue;
if (abs(j-i) == 0) {
tmpvec = makeOffterm(weight, P2[i], aTheta, ncat, i);
tmpvec2 = makeOffterm(weight, P[i], aTheta, ncat, i);
offterm = tmpvec - tmpvec2;
tmpvec = dat_num[i]*(aTheta*P[i] - 3*aTheta*P2[i] +
2*aTheta*P3[i]) * numsum - dat_num[i] *
(aTheta*P[i] - aTheta*P2[i])*numsum + weight[i] *
(P[i] - P2[i])*aTheta + offterm;
out[dkrow + nfact + i - 1] -= tmpvec;
} else {
offterm = makeOffterm2(weight, P[j], P[i], aTheta, ncat, i);
tmpvec = dat_num[i] * (-aTheta*P[i]*P[j] + 2*P2[i] *aTheta*P[j]) * numsum +
dat_num[i] * (P[i] - P2[i]) * aTheta * num[j] + offterm;
out[dkrow + nfact + i - 1] -= tmpvec;
}
}
}
}
void Model::MakeRaft(float &z)
{
vector<InFillHit> HitsBuffer;
uint LayerNr = 0;
float size = settings.Raft.Size;
Vector2f raftMin = Vector2f(Min.x - size + printOffset.x, Min.y - size + printOffset.y);
Vector2f raftMax = Vector2f(Max.x + size + printOffset.x, Max.y + size + printOffset.y);
Vector2f Center = (Vector2f(Max.x + size, Max.y + size)-Vector2f(Min.x + size, Min.y + size))/2+Vector2f(printOffset.x, printOffset.y);
float Length = sqrtf(2)*( ((raftMax.x)>(raftMax.y)? (raftMax.x):(raftMax.y)) - ((raftMin.x)<(raftMin.y)? (raftMin.x):(raftMin.y)) )/2.0f; // bbox of object
float E = 0.0f;
float rot;
while(LayerNr < settings.Raft.Phase[0].LayerCount + settings.Raft.Phase[1].LayerCount)
{
Settings::RaftSettings::PhasePropertiesType *props;
props = LayerNr < settings.Raft.Phase[0].LayerCount ?
&settings.Raft.Phase[0] : &settings.Raft.Phase[1];
rot = (props->Rotation+(float)LayerNr * props->RotationPrLayer)/180.0f*M_PI;
Vector2f InfillDirX(cosf(rot), sinf(rot));
Vector2f InfillDirY(-InfillDirX.y, InfillDirX.x);
Vector3f LastPosition;
bool reverseLines = false;
Vector2f P1, P2;
for(float x = -Length ; x < Length ; x+=props->Distance)
{
P1 = (InfillDirX * Length)+(InfillDirY*x) + Center;
P2 = (InfillDirX * -Length)+(InfillDirY*x) + Center;
if(reverseLines)
{
Vector2f tmp = P1;
P1 = P2;
P2 = tmp;
}
// glBegin(GL_LINES);
// glVertex2fv(&P1.x);
// glVertex2fv(&P2.x);
// Crop lines to bbox*size
Vector3f point;
InFillHit hit;
HitsBuffer.clear();
Vector2f P3(raftMin.x, raftMin.y);
Vector2f P4(raftMin.x, raftMax.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit)) //Intersect edges of bbox
HitsBuffer.push_back(hit);
P3 = Vector2f(raftMax.x,raftMax.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit))
HitsBuffer.push_back(hit);
P4 = Vector2f(raftMax.x,raftMin.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit))
HitsBuffer.push_back(hit);
P3 = Vector2f(raftMin.x,raftMin.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit))
HitsBuffer.push_back(hit);
// glEnd();
if(HitsBuffer.size() == 0) // it can only be 2 or zero
continue;
if(HitsBuffer.size() != 2)
continue;
std::sort(HitsBuffer.begin(), HitsBuffer.end(), InFillHitCompareFunc);
P1 = HitsBuffer[0].p;
P2 = HitsBuffer[1].p;
MakeAcceleratedGCodeLine (Vector3f(P1.x,P1.y,z), Vector3f(P2.x,P2.y,z),
props->MaterialDistanceRatio, gcode,
E, z, settings.Slicing, settings.Hardware);
reverseLines = !reverseLines;
}
// Set startspeed for Z-move
Command g;
g.Code = SETSPEED;
g.where = Vector3f(P2.x, P2.y, z);
g.f=settings.Hardware.MinPrintSpeedZ;
g.comment = "Move Z";
g.e = E;
gcode.commands.push_back(g);
z += props->Thickness * settings.Hardware.LayerThickness;
// Move Z
g.Code = ZMOVE;
g.where = Vector3f(P2.x, P2.y, z);
g.f = settings.Hardware.MinPrintSpeedZ;
g.comment = "Move Z";
g.e = E;
gcode.commands.push_back(g);
LayerNr++;
}
// restore the E state
Command gotoE;
gotoE.Code = GOTO;
gotoE.e = 0;
gotoE.comment = "Reset E for the remaining print";
gcode.commands.push_back(gotoE);
}
int P3P::computePoses(const Eigen::Matrix3d & feature_vectors, const Eigen::Matrix3d & world_points,
Eigen::Matrix<Eigen::Matrix<double, 3, 4>, 4, 1> & solutions)
{
// Extraction of world points
Eigen::Vector3d P1 = world_points.col(0);
Eigen::Vector3d P2 = world_points.col(1);
Eigen::Vector3d P3 = world_points.col(2);
// Verification that world points are not colinear
Eigen::Vector3d temp1 = P2 - P1;
Eigen::Vector3d temp2 = P3 - P1;
if (temp1.cross(temp2).norm() == 0)
{
return -1;
}
// Extraction of feature vectors
Eigen::Vector3d f1 = feature_vectors.col(0);
Eigen::Vector3d f2 = feature_vectors.col(1);
Eigen::Vector3d f3 = feature_vectors.col(2);
// Creation of intermediate camera frame
Eigen::Vector3d e1 = f1;
Eigen::Vector3d e3 = f1.cross(f2);
e3 = e3 / e3.norm();
Eigen::Vector3d e2 = e3.cross(e1);
Eigen::Matrix3d T;
T.row(0) = e1.transpose();
T.row(1) = e2.transpose();
T.row(2) = e3.transpose();
f3 = T * f3;
// Reinforce that f3(2,0) > 0 for having theta in [0;pi]
if (f3(2, 0) > 0)
{
f1 = feature_vectors.col(1);
f2 = feature_vectors.col(0);
f3 = feature_vectors.col(2);
e1 = f1;
e3 = f1.cross(f2);
e3 = e3 / e3.norm();
e2 = e3.cross(e1);
T.row(0) = e1.transpose();
T.row(1) = e2.transpose();
T.row(2) = e3.transpose();
f3 = T * f3;
P1 = world_points.col(1);
P2 = world_points.col(0);
P3 = world_points.col(2);
}
// Creation of intermediate world frame
Eigen::Vector3d n1 = P2 - P1;
n1 = n1 / n1.norm();
Eigen::Vector3d n3 = n1.cross(P3 - P1);
n3 = n3 / n3.norm();
Eigen::Vector3d n2 = n3.cross(n1);
Eigen::Matrix3d N;
N.row(0) = n1.transpose();
N.row(1) = n2.transpose();
N.row(2) = n3.transpose();
// Extraction of known parameters
P3 = N * (P3 - P1);
double d_12 = (P2 - P1).norm();
double f_1 = f3(0, 0) / f3(2, 0);
double f_2 = f3(1, 0) / f3(2, 0);
double p_1 = P3(0, 0);
double p_2 = P3(1, 0);
double cos_beta = f1.dot(f2);
double b = 1 / (1 - pow(cos_beta, 2)) - 1;
if (cos_beta < 0)
{
b = -sqrt(b);
}
else
{
b = sqrt(b);
}
// Definition of temporary variables for avoiding multiple computation
double f_1_pw2 = pow(f_1, 2);
double f_2_pw2 = pow(f_2, 2);
double p_1_pw2 = pow(p_1, 2);
double p_1_pw3 = p_1_pw2 * p_1;
double p_1_pw4 = p_1_pw3 * p_1;
double p_2_pw2 = pow(p_2, 2);
double p_2_pw3 = p_2_pw2 * p_2;
double p_2_pw4 = p_2_pw3 * p_2;
double d_12_pw2 = pow(d_12, 2);
double b_pw2 = pow(b, 2);
// Computation of factors of 4th degree polynomial
Eigen::Matrix<double, 5, 1> factors;
factors(0) = -f_2_pw2 * p_2_pw4 - p_2_pw4 * f_1_pw2 - p_2_pw4;
factors(1) = 2 * p_2_pw3 * d_12 * b + 2 * f_2_pw2 * p_2_pw3 * d_12 * b - 2 * f_2 * p_2_pw3 * f_1 * d_12;
factors(2) = -f_2_pw2 * p_2_pw2 * p_1_pw2 - f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2 - f_2_pw2 * p_2_pw2 * d_12_pw2
+ f_2_pw2 * p_2_pw4 + p_2_pw4 * f_1_pw2 + 2 * p_1 * p_2_pw2 * d_12 + 2 * f_1 * f_2 * p_1 * p_2_pw2 * d_12 * b
- p_2_pw2 * p_1_pw2 * f_1_pw2 + 2 * p_1 * p_2_pw2 * f_2_pw2 * d_12 - p_2_pw2 * d_12_pw2 * b_pw2
- 2 * p_1_pw2 * p_2_pw2;
factors(3) = 2 * p_1_pw2 * p_2 * d_12 * b + 2 * f_2 * p_2_pw3 * f_1 * d_12 - 2 * f_2_pw2 * p_2_pw3 * d_12 * b
- 2 * p_1 * p_2 * d_12_pw2 * b;
factors(4) = -2 * f_2 * p_2_pw2 * f_1 * p_1 * d_12 * b + f_2_pw2 * p_2_pw2 * d_12_pw2 + 2 * p_1_pw3 * d_12
- p_1_pw2 * d_12_pw2 + f_2_pw2 * p_2_pw2 * p_1_pw2 - p_1_pw4 - 2 * f_2_pw2 * p_2_pw2 * p_1 * d_12
+ p_2_pw2 * f_1_pw2 * p_1_pw2 + f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2;
// Computation of roots
Eigen::Matrix<double, 4, 1> realRoots;
P3P::solveQuartic(factors, realRoots);
// Backsubstitution of each solution
for (int i = 0; i < 4; ++i)
{
double cot_alpha = (-f_1 * p_1 / f_2 - realRoots(i) * p_2 + d_12 * b)
/ (-f_1 * realRoots(i) * p_2 / f_2 + p_1 - d_12);
double cos_theta = realRoots(i);
double sin_theta = sqrt(1 - pow((double)realRoots(i), 2));
double sin_alpha = sqrt(1 / (pow(cot_alpha, 2) + 1));
double cos_alpha = sqrt(1 - pow(sin_alpha, 2));
if (cot_alpha < 0)
{
cos_alpha = -cos_alpha;
}
Eigen::Vector3d C;
C(0) = d_12 * cos_alpha * (sin_alpha * b + cos_alpha);
C(1) = cos_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha);
C(2) = sin_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha);
C = P1 + N.transpose() * C;
Eigen::Matrix3d R;
R(0, 0) = -cos_alpha;
R(0, 1) = -sin_alpha * cos_theta;
R(0, 2) = -sin_alpha * sin_theta;
R(1, 0) = sin_alpha;
R(1, 1) = -cos_alpha * cos_theta;
R(1, 2) = -cos_alpha * sin_theta;
R(2, 0) = 0;
R(2, 1) = -sin_theta;
R(2, 2) = cos_theta;
R = N.transpose() * R.transpose() * T;
Eigen::Matrix<double, 3, 4> solution;
solution.block<3, 3>(0, 0) = R;
solution.col(3) = C;
solutions(i) = solution;
}
return 0;
}
CameraScreen::CameraScreen(Screen * screen, double screenDist)
: Camera(P3(screen->getWidth()/2, 0, screen->getHeight()/2), P3(0, 1, 0), 0) {
this->screen = screen;
this->screenDist = screenDist;
reinit();
}
int
/* main(int argc, char *argv[]) */
whetstone_main()
{
/* used in the FORTRAN version */
long I;
long N1, N2, N3, N4, N6, N7, N8, N9, N10, N11;
double X1,X2,X3,X4,X,Y,Z;
long LOOP;
int II, JJ;
/* added for this version */
long loopstart;
long long startsec, finisec;
float KIPS;
int continuous;
loopstart = 1000000; /* see the note about LOOP below */
loopstart = 250000;
continuous = 0;
II = 1; /* start at the first arg (temp use of II here) */
/* while (II < argc) { */
/* if (strncmp(argv[II], "-c", 2) == 0 || argv[II][0] == 'c') { */
/* continuous = 1; */
/* } else if (atol(argv[II]) > 0) { */
/* loopstart = atol(argv[II]); */
/* } else { */
/* fprintf(stderr, USAGE); */
/* return(1); */
/* } */
/* II++; */
/* } */
LCONT:
/*
C
C Start benchmark timing at this point.
C
*/
startsec = get_microsec();// time(0);
/*
C
C The actual benchmark starts here.
C
*/
T = .499975;
T1 = 0.50025;
T2 = 2.0;
/*
C
C With loopcount LOOP=10, one million Whetstone instructions
C will be executed in EACH MAJOR LOOP..A MAJOR LOOP IS EXECUTED
C 'II' TIMES TO INCREASE WALL-CLOCK TIMING ACCURACY.
C
LOOP = 1000;
*/
LOOP = loopstart;
II = 1;
JJ = 1;
IILOOP:
N1 = 0;
N2 = 12 * LOOP;
N3 = 14 * LOOP;
N4 = 345 * LOOP;
N6 = 210 * LOOP;
N7 = 32 * LOOP;
N8 = 899 * LOOP;
N9 = 616 * LOOP;
N10 = 0;
N11 = 93 * LOOP;
/*
C
C Module 1: Simple identifiers
C
*/
X1 = 1.0;
X2 = -1.0;
X3 = -1.0;
X4 = -1.0;
for (I = 1; I <= N1; I++) {
X1 = (X1 + X2 + X3 - X4) * T;
X2 = (X1 + X2 - X3 + X4) * T;
X3 = (X1 - X2 + X3 + X4) * T;
X4 = (-X1+ X2 + X3 + X4) * T;
}
#ifdef PRINTOUT
IF (JJ==II)POUT(N1,N1,N1,X1,X2,X3,X4);
#endif
/*
C
C Module 2: Array elements
C
*/
E1[1] = 1.0;
E1[2] = -1.0;
E1[3] = -1.0;
E1[4] = -1.0;
for (I = 1; I <= N2; I++) {
E1[1] = ( E1[1] + E1[2] + E1[3] - E1[4]) * T;
E1[2] = ( E1[1] + E1[2] - E1[3] + E1[4]) * T;
E1[3] = ( E1[1] - E1[2] + E1[3] + E1[4]) * T;
E1[4] = (-E1[1] + E1[2] + E1[3] + E1[4]) * T;
}
#ifdef PRINTOUT
IF (JJ==II)POUT(N2,N3,N2,E1[1],E1[2],E1[3],E1[4]);
#endif
/*
C
C Module 3: Array as parameter
C
*/
for (I = 1; I <= N3; I++)
PA(E1);
#ifdef PRINTOUT
IF (JJ==II)POUT(N3,N2,N2,E1[1],E1[2],E1[3],E1[4]);
#endif
/*
C
C Module 4: Conditional jumps
C
*/
J = 1;
for (I = 1; I <= N4; I++) {
if (J == 1)
J = 2;
else
J = 3;
if (J > 2)
J = 0;
else
J = 1;
if (J < 1)
J = 1;
else
J = 0;
}
#ifdef PRINTOUT
IF (JJ==II)POUT(N4,J,J,X1,X2,X3,X4);
#endif
/*
C
C Module 5: Omitted
C Module 6: Integer arithmetic
C
*/
J = 1;
K = 2;
L = 3;
for (I = 1; I <= N6; I++) {
J = J * (K-J) * (L-K);
K = L * K - (L-J) * K;
L = (L-K) * (K+J);
E1[L-1] = J + K + L;
E1[K-1] = J * K * L;
}
#ifdef PRINTOUT
IF (JJ==II)POUT(N6,J,K,E1[1],E1[2],E1[3],E1[4]);
#endif
/*
C
C Module 7: Trigonometric functions
C
*/
X = 0.5;
Y = 0.5;
for (I = 1; I <= N7; I++) {
X = T * DATAN(T2*DSIN(X)*DCOS(X)/(DCOS(X+Y)+DCOS(X-Y)-1.0));
Y = T * DATAN(T2*DSIN(Y)*DCOS(Y)/(DCOS(X+Y)+DCOS(X-Y)-1.0));
}
#ifdef PRINTOUT
IF (JJ==II)POUT(N7,J,K,X,X,Y,Y);
#endif
/*
C
C Module 8: Procedure calls
C
*/
X = 1.0;
Y = 1.0;
Z = 1.0;
for (I = 1; I <= N8; I++)
P3(X,Y,&Z);
#ifdef PRINTOUT
IF (JJ==II)POUT(N8,J,K,X,Y,Z,Z);
#endif
/*
C
C Module 9: Array references
C
*/
J = 1;
K = 2;
L = 3;
E1[1] = 1.0;
E1[2] = 2.0;
E1[3] = 3.0;
for (I = 1; I <= N9; I++)
P0();
#ifdef PRINTOUT
IF (JJ==II)POUT(N9,J,K,E1[1],E1[2],E1[3],E1[4]);
#endif
/*
C
C Module 10: Integer arithmetic
C
*/
J = 2;
K = 3;
for (I = 1; I <= N10; I++) {
J = J + K;
K = J + K;
J = K - J;
K = K - J - J;
}
#ifdef PRINTOUT
IF (JJ==II)POUT(N10,J,K,X1,X2,X3,X4);
#endif
/*
C
C Module 11: Standard functions
C
*/
X = 0.75;
for (I = 1; I <= N11; I++)
X = DSQRT(DEXP(DLOG(X)/T1));
#ifdef PRINTOUT
IF (JJ==II)POUT(N11,J,K,X,X,X,X);
#endif
/*
C
C THIS IS THE END OF THE MAJOR LOOP.
C
*/
if (++JJ <= II)
goto IILOOP;
/*
C
C Stop benchmark timing at this point.
C
*/
finisec = get_microsec();// time(0);
/*
C----------------------------------------------------------------
C Performance in Whetstone KIP's per second is given by
C
C (100*LOOP*II)/TIME
C
C where TIME is in seconds.
C--------------------------------------------------------------------
*/
_printf("\n");
if (finisec-startsec <= 0) {
_printf("Insufficient duration- Increase the LOOP count\n");
return(1);
}
_printf("Loops: %ld, Iterations: %d, Duration: %lld sec.\n",
LOOP, II, (finisec-startsec)/1000000);
KIPS = (100.0*LOOP*II)/((double)(finisec-startsec)/1000000.0); //(((double)(finisec-startsec)) / (double)CLOCKS_PER_SEC );
if (KIPS >= 1000.0){
_printf("C Converted Double Precision Whetstones: %.1f MIPS\n", KIPS/1000.0);
}
else{
_printf("C Converted Double Precision Whetstones: %.1f KIPS\n", KIPS);
}
if (continuous)
goto LCONT;
return(0);
}
void ProcessController::MakeRaft(float &z)
{
vector<InFillHit> HitsBuffer;
uint LayerNr = 0;
float step;
float size = RaftSize;
Vector2f raftMin = Vector2f(Min.x - size+printOffset.x, Min.y - size+printOffset.y);
Vector2f raftMax = Vector2f(Max.x + size+printOffset.x, Max.y + size+printOffset.y);
Vector2f Center = (Vector2f(Max.x + size, Max.y + size)-Vector2f(Min.x + size, Min.y + size))/2+Vector2f(printOffset.x, printOffset.y);
float Length = sqrtf(2)*( ((raftMax.x)>(raftMax.y)? (raftMax.x):(raftMax.y)) - ((raftMin.x)<(raftMin.y)? (raftMin.x):(raftMin.y)) )/2.0f; // bbox of object
float E = 0.0f;
float rot = RaftRotation/180.0f*M_PI;
while(LayerNr < RaftBaseLayerCount+RaftInterfaceLayerCount)
{
rot = (RaftRotation+(float)LayerNr*RaftRotationPrLayer)/180.0f*M_PI;
Vector2f InfillDirX(cosf(rot), sinf(rot));
Vector2f InfillDirY(-InfillDirX.y, InfillDirX.x);
Vector3f LastPosition;
bool reverseLines = false;
if(LayerNr < RaftBaseLayerCount)
step = RaftBaseDistance;
else
step = RaftInterfaceDistance;
Vector2f P1, P2;
for(float x = -Length ; x < Length ; x+=step)
{
P1 = (InfillDirX * Length)+(InfillDirY*x)+ Center;
P2 = (InfillDirX * -Length)+(InfillDirY*x)+ Center;
if(reverseLines)
{
Vector2f tmp = P1;
P1 = P2;
P2 = tmp;
}
// glBegin(GL_LINES);
// glVertex2fv(&P1.x);
// glVertex2fv(&P2.x);
// Crop lines to bbox*size
Vector3f point;
InFillHit hit;
HitsBuffer.clear();
Vector2f P3(raftMin.x, raftMin.y);
Vector2f P4(raftMin.x, raftMax.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit)) //Intersect edges of bbox
HitsBuffer.push_back(hit);
P3 = Vector2f(raftMax.x,raftMax.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit))
HitsBuffer.push_back(hit);
P4 = Vector2f(raftMax.x,raftMin.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit))
HitsBuffer.push_back(hit);
P3 = Vector2f(raftMin.x,raftMin.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit))
HitsBuffer.push_back(hit);
// glEnd();
if(HitsBuffer.size() == 0) // it can only be 2 or zero
continue;
if(HitsBuffer.size() != 2)
continue;
std::sort(HitsBuffer.begin(), HitsBuffer.end(), InFillHitCompareFunc);
P1 = HitsBuffer[0].p;
P2 = HitsBuffer[1].p;
float materialRatio;
if(LayerNr < RaftBaseLayerCount)
materialRatio = RaftMaterialPrDistanceRatio; // move or extrude?
else
materialRatio = RaftInterfaceMaterialPrDistanceRatio; // move or extrude?
MakeAcceleratedGCodeLine(Vector3f(P1.x,P1.y,z), Vector3f(P2.x,P2.y,z), DistanceToReachFullSpeed, materialRatio, gcode, z, MinPrintSpeedXY, MaxPrintSpeedXY, MinPrintSpeedZ, MaxPrintSpeedZ, UseIncrementalEcode, Use3DGcode, E, EnableAcceleration);
reverseLines = !reverseLines;
}
// Set startspeed for Z-move
Command g;
g.Code = SETSPEED;
g.where = Vector3f(P2.x, P2.y, z);
g.f=MinPrintSpeedZ;
g.comment = "Move Z";
g.e = E;
gcode.commands.push_back(g);
if(LayerNr < RaftBaseLayerCount)
z+=RaftBaseThickness*LayerThickness;
else
z+=RaftInterfaceThickness*LayerThickness;
// Move Z
g.Code = ZMOVE;
g.where = Vector3f(P2.x, P2.y, z);
g.f=MinPrintSpeedZ;
g.comment = "Move Z";
g.e = E;
gcode.commands.push_back(g);
LayerNr++;
}
}
bool Rectangle::Intersect(const Ray &r, float *tHit, float *rayEpsilon,
DifferentialGeometry *dg) const {
// Transform _Ray_ to object space
Ray ray;
(*WorldToObject)(r, &ray);
// Compute plane intersection for disk
// Checks if the plane is parallel to the ray or not
// We can get the direction of the ray
// If the Z component of the direction of the ray is zero
// then, the ray is parallel to the plane and in such case
// there is no intersection point between the ray and the plane.
if (fabsf(ray.d.z) < 1e-7)
return false;
// Now, the direction of the ray is not parallel to the plane
// We have to check if the intersection happens or not
// We have to compute the parametric t where the ray intersects the plane
// We want to find t such that the z-component of the ray intersects the plane
// The ray "line" equation is l = l0 + (l1 - l0) * t
// l1 - l0 will give us the distance between the two points on the plane
// Then t is the ratio and in such case it should be between 0 and 1
// Considering that the rectangle completely lies in the z plane
/// distance = l1 - l0
/// thit = (l - l0) / distance
// But since we assume that the plane is located at height
// Then, the point l is at height on the plane
/// l = height
float thit = (height - ray.o.z) / ray.d.z;
// Then we check if the thit is between the ratio of 0 and 1 that is mapped
// between ray.mint and ray.maxt, if not retrun false
if (thit < ray.mint || thit > ray.maxt)
return false;
// Then we see if the point lies inside the disk or not
// Substitute the thit in the ray equation to get hit point on the ray
Point phit = ray(thit);
// We have to make sure that the interesction lies inside the plane
if (!(phit.x < x/2 && phit.x > -x/2 && phit.y < y/2 && phit.y > -y/2))
return false;
// Assuming that the plane is formed from the following 4 points
// P0, P1, P2, P3
//
// p0 *---------------* p1
// | |
// | |
// | |
// | O |
// | |
// | |
// | |
// p2 *---------------* p3 -> X
//
// P0 @ (-x/2, y/2)
// P1 @ (x/2, y/2)
// P2 @ (-x/2, -y/2)
// P3 @ (x/2, -y/2)
Point P0(-x/2, y/2, height), P1(x/2, y/2, height);
Point P2(-x/2, -y/2, height), P3(x/2, -y/2, height);
/// Now, we have to find the parametric form of the plane in terms of (u,v)
/// Plane equation can be formed by at least 3 points P0, P1, P2
/// P0 -> P1 (vector 1)
/// P0 -> p2 (vector 2)
/// An arbitrary point on the plane p is found in the following parametric form
/// P = P0 + (P1 - P0) u + (P2 - P0) v
/// Now we need to express two explicit equations of u and v
/// So, we have to construct the system of equation that solves for u and v
///
/// Since we have found the intersection point between the plane and the line
/// we have to use it to formalize the system of equations that will be used
/// to find the parametric form of the plane
/// Plane equation is : P = P0 + (P1 - P0) u + (P2 - P0) v
/// Ray equation is : l = l0 + (l1 - l0) * thit
/// But l = P, then
/// l0 + (l1 - l0) * thit = P0 + (P1 - P0) * u + (P2 - P0) * v
/// l0 - P0 = (l0 - l1) * thit + (P1 - P0) * u + (P2 - P0) * v
/// MAPPING : l0 = ray.o
/// [l0.x - P0.x] = [l0.x - l1.x P1.x - P0.x P2.x - P0.x] [t]
/// [l0.y - P0.y] = [l0.y - l1.y P1.y - P0.y P2.y - P0.y] [u]
/// [l0.z - P0.z] = [l0.z - l1.z P1.z - P0.z P2.z - P0.z] [v]
///
/// Then, we should find the inverse of the matrix in order to
/// solve for u,v and t for check
// System AX = B
float a11 = ray.o.x - 0;
float a12 = P1.x - P0.x;
float a13 = P2.x - P0.x;
float a21 = ray.o.y - 0;
float a22 = P1.y - P0.y;
float a23 = P2.y - P0.y;
float a31 = ray.o.y - height;
float a32 = P1.z - P0.z;
float a33 = P2.z - P0.z;
float b1 = -7;
float b2 = -2;
float b3 = 14;
float x1 = 0;
float x2 = 0;
float x3 = 0;
Imath::M33f A(a11,a12,a13,a21,a22,a23,a31,a32,a33), AInverted;
Imath::V3f X(x1, x2, x3);
Imath::V3f B(b1,b2, b3);
// This operation has been checked and working for getting
// the correct inverse of the matrix A
AInverted = A.invert(false);
x1 = AInverted[0][0] * B[0] +
AInverted[0][1] * B[1] +
AInverted[0][2] * B[2];
x2 = AInverted[1][0] * B[0] +
AInverted[1][1] * B[1] +
AInverted[1][2] * B[2];
x3 = AInverted[2][0] * B[0] +
AInverted[2][1] * B[1] +
AInverted[2][2] * B[2];
/// Then we have u = something, and v = something
///
/// Then we come for the derivatives, so we have to find the derivatives
/// from the parametric forms defined above for the plane equations
/// dpdu = (P1 - P0)
/// dpdv = (P2 - P0)
///
/// For the normal we have the always fixed direction in y
/// So the derivative for the normal is zero
/// dndu = (0, 0, 0) dndv = (0, 0, 0)
///
/// Then we can construct the DifferentilGeometry and go ahead
// Find parametric representation of disk hit
float u = x2;
float v = x3;
Vector dpdu(P1.x - P0.x, P1.y - P0.y, P1.z - P0.z);
Vector dpdv(P2.x - P0.x, P2.y - P0.y, P2.z - P0.z);
Normal dndu(0,0,0), dndv(0,0,0);
// Initialize _DifferentialGeometry_ from parametric information
const Transform &o2w = *ObjectToWorld;
*dg = DifferentialGeometry(o2w(phit), o2w(dpdu), o2w(dpdv),
o2w(dndu), o2w(dndv), u, v, this);
// Update _tHit_ for quadric intersection
*tHit = thit;
// Compute _rayEpsilon_ for quadric intersection
*rayEpsilon = 5e-4f * *tHit;
return true;
}
