void _inplace_glify(GLfloat* v)
{
hmatrix xmat;
hvector rvec;
real32 deg = -90.0f;
real32 deg2 = -90.0f;
hmatMakeRotAboutZ(&xmat, fcos(DEG_TO_RAD(deg)), fsin(DEG_TO_RAD(deg)));
hmatMultiplyHVecByHMat(&rvec, (hvector*)v, &xmat);
if (v[3] != 0.0f)
{
v[0] /= v[3];
v[1] /= v[3];
v[2] /= v[3];
}
memcpy(v, &rvec, 3*sizeof(real32));
hmatMakeRotAboutX(&xmat, fcos(DEG_TO_RAD(deg2)), fsin(DEG_TO_RAD(deg2)));
hmatMultiplyHVecByHMat(&rvec, (hvector*)v, &xmat);
if (v[3] != 0.0f)
{
v[0] /= v[3];
v[1] /= v[3];
v[2] /= v[3];
}
memcpy(v, &rvec, 3*sizeof(real32));
}
/*-----------------------------------------------------------------------------
Name : btgConvertAVert
Description : spherically unprojects a vert
Inputs : out - target output vector
x, y - 2D position
Outputs : out is modified
Return :
----------------------------------------------------------------------------*/
void btgConvertAVert(math::Vec3f* out, real64 x, real64 y)
{
real32 xFrac, yFrac;
real32 theta, phi;
real32 sinTheta, cosTheta;
real32 sinPhi, cosPhi;
real32 pageWidth, pageHeight;
real32 radius;
pageWidth = (real32)btgHead->pageWidth;
pageHeight = (real32)btgHead->pageHeight;
x = ((real64)pageWidth - 1.0) - x;
xFrac = (real32)x / pageWidth;
yFrac = (real32)y / pageHeight;
theta = 2.0f * M_PI_F * xFrac;
phi = 1.0f * M_PI_F * yFrac;
theta += math::degToRad(btgThetaOffset) + math::degToRad(90.0f);
phi += math::degToRad(btgPhiOffset);
sinTheta = fsin(theta);
cosTheta = fcos(theta);
sinPhi = fsin(phi);
cosPhi = fcos(phi);
radius = 1.0;
out->x = radius * cosTheta * sinPhi;
out->y = radius * sinTheta * sinPhi;
out->z = radius * cosPhi;
}
/* draw_object just draws a single object at a fixed position, although
this can easily be modified to allow for more objects.
bmp = bitmap to draw to. obj = sprite for the object.
angle, cx, cy define the camera position.
*/
void draw_object (BITMAP *bmp, BITMAP *obj, fixed angle, fixed cx, fixed cy, MODE_7_PARAMS params)
{
int width, height;
int screen_y, screen_x;
// The object in this case is at a fixed position of (160, 100).
// Calculate the position relative to the camera.
fixed obj_x = itofix(160) - cx;
fixed obj_y = itofix(100) - cy;
// use a rotation transformation to rotate the object by the camera
// angle
fixed space_x = fmul (obj_x, fcos (angle)) + fmul (obj_y, fsin (angle));
fixed space_y = -fmul (obj_x, fsin (angle)) + fmul (obj_y, fcos (angle));
// calculate the screen coordinates that go with these space coordinates
// by dividing everything by space_x (the distance)
screen_x = bmp->w/2 + fixtoi (fmul (fdiv (params.scale_x, space_x), space_y));
screen_y = fixtoi (fdiv (fmul (params.space_z, params.scale_y), space_x)) - params.horizon;
// the size of the object has to be scaled according to the distance
height = fixtoi (obj->h * fdiv(params.obj_scale_y, space_x));
width = fixtoi (obj->w * fdiv(params.obj_scale_x, space_x));
// draw the object
stretch_sprite (bmp, obj, screen_x - width / 2, screen_y - height, width, height);
}
/*-----------------------------------------------------------------------------
Name : btgConvertAVert
Description : spherically unprojects a vert
Inputs : out - target output vector
x, y - 2D position
Outputs : out is modified
Return :
----------------------------------------------------------------------------*/
void btgConvertAVert(vector* out, real32 x, real32 y)
{
real32 xFrac, yFrac;
real32 theta, phi;
real32 sinTheta, cosTheta;
real32 sinPhi, cosPhi;
real32 pageWidth, pageHeight;
real32 radius;
pageWidth = (real32)btgHead->pageWidth;
pageHeight = (real32)btgHead->pageHeight;
x = ((real32)pageWidth - 1.0) - x;
xFrac = (real32)x / pageWidth;
yFrac = (real32)y / pageHeight;
theta = 2.0f * M_PI_F * xFrac;
phi = 1.0f * M_PI_F * yFrac;
theta += DEG_TO_RAD(btgThetaOffset) + DEG_TO_RAD(90.0f);
phi += DEG_TO_RAD(btgPhiOffset);
sinTheta = fsin(theta);
cosTheta = fcos(theta);
sinPhi = fsin(phi);
cosPhi = fcos(phi);
radius = CAMERA_CLIP_FAR - 500.0f;
out->x = radius * cosTheta * sinPhi;
out->y = radius * sinTheta * sinPhi;
out->z = radius * cosPhi;
}
void
flat_face(float ir, float or, float wd)
{
int i;
float w;
/* draw each face (top & bottom ) * */
if (poo)
printf("Face : %f..%f wid=%f\n", ir, or, wd);
if (wd == 0.0)
return;
for (w = wd / 2; w > -wd; w -= wd) {
if (w > 0.0)
glNormal3f(0.0, 0.0, 1.0);
else
glNormal3f(0.0, 0.0, -1.0);
if (ir == 0.0) {
/* draw as t-fan */
glBegin(GL_TRIANGLE_FAN);
glVertex3f(0.0, 0.0, w); /* center */
glVertex3f(or, 0.0, w);
for (i = 1; i < circle_subdiv; i++) {
glVertex3f(fcos(2.0 * M_PI * i / circle_subdiv) * or,
fsin(2.0 * M_PI * i / circle_subdiv) * or,
w);
}
glVertex3f(or, 0.0, w);
glEnd();
} else {
/* draw as tmesh */
glBegin(GL_TRIANGLE_STRIP);
glVertex3f(or, 0.0, w);
glVertex3f(ir, 0.0, w);
for (i = 1; i < circle_subdiv; i++) {
glVertex3f(fcos(2.0 * M_PI * i / circle_subdiv) * or,
fsin(2.0 * M_PI * i / circle_subdiv) * or,
w);
glVertex3f(fcos(2.0 * M_PI * i / circle_subdiv) * ir,
fsin(2.0 * M_PI * i / circle_subdiv) * ir,
w);
}
glVertex3f(or, 0.0, w);
glVertex3f(ir, 0.0, w);
glEnd();
}
}
}
void mode_7 (BITMAP *bmp, BITMAP *tile, fixed angle, fixed cx, fixed cy, MODE_7_PARAMS params)
{
// current screen position
int screen_x, screen_y;
// the distance and horizontal scale of the line we are drawing
fixed distance, horizontal_scale;
// masks to make sure we don't read pixels outside the tile
int mask_x = (tile->w - 1);
int mask_y = (tile->h -1);
// step for points in space between two pixels on a horizontal line
fixed line_dx, line_dy;
// current space position
fixed space_x, space_y;
for (screen_y = 0; screen_y < bmp->h; screen_y++)
{
// first calculate the distance of the line we are drawing
distance = fdiv (fmul (params.space_z, params.scale_y),
itofix (screen_y + params.horizon));
// then calculate the horizontal scale, or the distance between
// space points on this horizontal line
horizontal_scale = fdiv (distance, params.scale_x);
// calculate the dx and dy of points in space when we step
// through all points on this line
line_dx = fmul (-fsin(angle), horizontal_scale);
line_dy = fmul (fcos(angle), horizontal_scale);
// calculate the starting position
space_x = cx + fmul (distance, fcos(angle)) - bmp->w/2 * line_dx;
space_y = cy + fmul (distance, fsin(angle)) - bmp->w/2 * line_dy;
// go through all points in this screen line
for (screen_x = 0; screen_x < bmp->w; screen_x++)
{
// get a pixel from the tile and put it on the screen
putpixel (bmp, screen_x, screen_y,
getpixel (tile,
fixtoi (space_x) & mask_x,
fixtoi (space_y) & mask_y ));
// advance to the next position in space
space_x += line_dx;
space_y += line_dy;
}
}
}
void matrix4::perspective(float fovy, float aspect, float zNear, float zFar, bool infinite)
{
double radians = fovy / 2 * (3.14159265358979323846) / 180;
double cotangent = fcos(radians) / fsin(radians);
float deltaZ = zFar - zNear;
m[0] = (float)(cotangent / aspect);
m[1] = 0.0f;
m[2] = 0.0f;
m[3] = 0.0f;
m[4] = 0.0f;
m[5] = (float)cotangent;
m[6] = 0.0f;
m[7] = 0.0f;
m[8] = 0.0f;
m[9] = 0.0f;
m[10] = -(zFar + zNear) / deltaZ;
m[11] = -1.0f;
m[12] = 0.0f;
m[13] = 0.0f;
m[14] = -2 * zNear * zFar / deltaZ;
m[15] = 0.0f;
if (infinite)
{
m[10] = -1.0f;
m[14] = -2.0f;
}
}
void main()
{
init_sincos_library();
printf("sine: %f\n",fsin(M_PI));
printf("cosine: %f\n",fcos(M_PI));
close_sincos_library();
}
void walk_backward() {
camera_x += (fsin(camera_yrot)*rad_to_deg) * 0.002f;
camera_z -= (fcos(camera_yrot)*rad_to_deg) * 0.002f;
walkbiasangle-= 10;
walkbiasangle%=360;
walkbias=fsin(walkbiasangle * piover180)*0.25f;
}
static void
update_IrPoints( irInfo* irs)
{
int cnt;
Point rpos;
float rrot;
float distanceToObstacle = ROB_RADIUS + IR_THRESHOLD_DIST;
updateActualPosition( &rpos, &rrot, DONT_CONSIDER_DIRECTION);
for ( cnt = 0; cnt < irs->numberOfActivatedIrs; cnt++) {
float angle = rrot + irs->angle[cnt];
Ir_Obstacle_Points.points[cnt].x =
rpos.x + (fcos( angle) * distanceToObstacle);
Ir_Obstacle_Points.points[cnt].y =
rpos.y + (fsin( angle) * distanceToObstacle);
}
if ( irs->numberOfActivatedIrs > 0)
setStartTime( IR_TIMER);
Ir_Obstacle_Points.no_of_points = irs->numberOfActivatedIrs;
}
void strafe_right() {
camera_x += fsin(camera_yrot+(90*piover180)) * 0.1f;
camera_z += fcos(camera_yrot+(90*piover180)) * 0.1f;
walkbiasangle-= 10;
walkbiasangle%=360;
walkbias=fsin(walkbiasangle * piover180)*0.25f;
}
static int grproc_xadvance( INSTANCE * my, int * params )
{
int angle = params[0] ;
LOCINT32( mod_grproc, my, COORDX ) += fixtoi( fmul( fcos( angle ), itofix( params[1] ) ) ) ;
LOCINT32( mod_grproc, my, COORDY ) -= fixtoi( fmul( fsin( angle ), itofix( params[1] ) ) ) ;
return 1 ;
}
// readjust the settings, call this when slide dimension is changed
void PictureFlowState::reposition() {
angle = 70 * IANGLE_MAX / 360; // approx. 70 degrees tilted
offsetX = slideWidth / 2 * (PFREAL_ONE - fcos(angle));
offsetY = slideWidth / 2 * fsin(angle);
offsetX += slideWidth * PFREAL_ONE;
offsetY += slideWidth * PFREAL_ONE / 4;
spacing = 40;
}
void alienrocket::update(){
if(fired){
// rocket moves towards destination:
if((fixtoi(x) > 0) && (fixtoi(x) < 800) && (fixtoi(y) < 600) && (fixtoi(y) > 0)){
x += speed * fcos (angle);
y += speed * fsin (angle);
}else{ fired = false; }
//angle = (angle + angle_stepsize) & 0xFFFFFF;
//if we hit the player
if (abs (x - targetX) + abs (y - targetY) < itofix(50)){ hit = true; fired = false; }
}
}
// T and Q defines the light source as polar coordinates where
// T is the elevation angle ranging from 0 to Pi/2 (90 degrees) and
// Q is the rotation angle ranging from 0 to Pi*2 (360 degrees).
// h is an intensity value specifying how much bumping should be done. 0 <= h <= 1
void
set_bump_direction (float T, float Q, float h)
{
unsigned char Q2 = (unsigned char) ((Q / (2 * 3.1415927))* 255);
unsigned char h2 = (unsigned char) (h * 255);
unsigned char k1 = 255 - h2;
unsigned char k2 = (unsigned char) (h2 * fsin(T));
unsigned char k3 = (unsigned char) (h2 * fcos(T));
/* The bump direction is encoded as an offset colour. */
glKosOffsetColor4ub (k2, k3, Q2, k1);
}
// readjust the settings, call this when slide dimension is changed
void PictureFlowState::reposition()
{
// angle = 70 * IANGLE_MAX / 360; // approx. 70 degrees tilted
angle = rawAngle * IANGLE_MAX / 360;
offsetX = slideWidth/2 * (PFREAL_ONE-fcos(angle));
offsetY = slideWidth/2 * fsin(angle);
offsetX += slideWidth * PFREAL_ONE;
offsetY += slideWidth * PFREAL_ONE / 3;
if(rawAngle < 45)
offsetX += offsetX/4;
if(angle>0)
spacing = slideWidth * 0.35;
else
spacing = slideWidth*spacingRatio + slideWidth*(spacingRatio?0.10:0.2);
}
void Matrix::rotate(float angle, const Vector & axis) {
float vx = axis.x, vy = axis.y, vz = axis.z;
float rcos = fcos(angle * DEG2RAD);
float rsin = fsin(angle * DEG2RAD);
float invrcos = (1.0f - rcos);
float mag = fsqrt(vx*vx + vy*vy + vz*vz);
float xx, yy, zz, xy, yz, zx;
if (mag < 1.0e-6) {
// Rotation vector is too small to be significant
return;
}
// Normalize the rotation vector
vx /= mag;
vy /= mag;
vz /= mag;
xx = vx * vx;
yy = vy * vy;
zz = vz * vz;
xy = (vx * vy * invrcos);
yz = (vy * vz * invrcos);
zx = (vz * vx * invrcos);
// Generate the rotation matrix
Matrix mr; mr.identity();
// Hehe
mr.matrix[0][0] = xx + rcos * (1.0f - xx);
mr.matrix[2][1] = yz - vx * rsin;
mr.matrix[1][2] = yz + vx * rsin;
mr.matrix[1][1] = yy + rcos * (1.0f - yy);
mr.matrix[2][0] = zx + vy * rsin;
mr.matrix[0][2] = zx - vy * rsin;
mr.matrix[2][2] = zz + rcos * (1.0f - zz);
mr.matrix[1][0] = xy - vz * rsin;
mr.matrix[0][1] = xy + vz * rsin;
// Multiply it onto us
*this = *this * mr;
}
vec3 quaternion::rotate(float delta, vec3 axis, vec3 vector)
{
quaternion v(0.0f, vector);
quaternion result_quaternion;
vec3 result_vector;
float cosval = (float)fcos(delta / 2);
float sinval = (float)fsin(delta / 2);
s = cosval;
x = sinval * axis.x;
y = sinval * axis.y;
z = sinval * axis.z;
quaternion qinv(s, -x, -y, -z);
result_quaternion = (*this * v * qinv);
result_vector.x = result_quaternion.x;
result_vector.y = result_quaternion.y;
result_vector.z = result_quaternion.z;
return result_vector;
}
double max_fcos_error() {
double x;
double max_error_x = 0;
double max_error = fcos_error(0);
double error;
for (x = 0; x <= M_PI; x += M_PI/100000) {
error = fcos_error(x);
if (error > max_error) {
max_error_x = x;
max_error = error;
}
}
printf("Max error is %f\nfcos(%f) = %f\n cos(%f) = %f\n",
max_error, max_error_x, fcos(max_error_x), max_error_x, cos(max_error_x));
printf("Residuo obtido analiticamente: %f\n", fabs(-pow(M_PI/2, 5)/120));
return max_error;
}
void gmiscMatrixFloat2DApplyRotation(MatrixFloat2D *dst, const MatrixFloat2D *src, int angle) {
float s, c;
#if GMISC_NEED_FASTTRIG
s = fsin(angle);
c = fcos(angle);
#else
c = angle*M_PI/180;
s = sin(c);
c = cos(c);
#endif
if (src) {
dst->a00 = src->a00*c - src->a01*s; dst->a01 = src->a00*s + src->a01*c; dst->a02 = src->a02;
dst->a10 = src->a10*c - src->a11*s; dst->a11 = src->a10*s + src->a11*c; dst->a12 = src->a12;
dst->a20 = src->a20*c - src->a21*s; dst->a21 = src->a20*s + src->a21*c; dst->a22 = src->a22;
} else {
dst->a00 = c; dst->a01 = s; dst->a02 = 0.0;
dst->a10 = -s; dst->a11 = c; dst->a12 = 0.0;
dst->a20 = 0.0; dst->a21 = 0.0; dst->a22 = 1.0;
}
}
void
draw_outside(float w1, float w2, float rad)
{
int i, j;
float c, s;
if (poo)
printf("Outsid: wid=%f..%f rad=%f\n", w1, w2, rad);
if (w1 == w2)
return;
w1 = w1 / 2;
w2 = w2 / 2;
for (j = 0; j < 2; j++) {
if (j == 1) {
w1 = -w1;
w2 = -w2;
}
glBegin(GL_TRIANGLE_STRIP);
glNormal3f(1.0, 0.0, 0.0);
glVertex3f(rad, 0.0, w1);
glVertex3f(rad, 0.0, w2);
for (i = 1; i < circle_subdiv; i++) {
c = fcos(2.0 * M_PI * i / circle_subdiv);
s = fsin(2.0 * M_PI * i / circle_subdiv);
glNormal3f(c, s, 0.0);
glVertex3f(c * rad,
s * rad,
w1);
glVertex3f(c * rad,
s * rad,
w2);
}
glNormal3f(1.0, 0.0, 0.0);
glVertex3f(rad, 0.0, w1);
glVertex3f(rad, 0.0, w2);
glEnd();
}
}
// create a globe with constant radius and color
void miniglobe::create_globe(float radius,const float color[3])
{
int i,j;
const int alpha_steps=4*STRIPES;
const int beta_steps=STRIPES;
float u,v;
float alpha,beta;
float nx,ny,nz;
STRIP->setcol(color[0],color[1],color[2]);
for (j=-beta_steps; j<beta_steps; j++)
{
STRIP->beginstrip();
for (i=alpha_steps; i>=0; i--)
{
u=(float)i/alpha_steps;
v=(float)j/beta_steps;
alpha=u*2*PI;
beta=v*PI/2;
nx=fcos(alpha)*fcos(beta);
nz=-fsin(alpha)*fcos(beta);
ny=fsin(beta);
STRIP->setnrm(nx,ny,nz);
STRIP->settex(u,0.5f-v/2);
STRIP->addvtx(nx*radius,ny*radius,nz*radius);
v=(float)(j+1)/beta_steps;
beta=v*PI/2;
nx=fcos(alpha)*fcos(beta);
nz=-fsin(alpha)*fcos(beta);
ny=fsin(beta);
STRIP->setnrm(nx,ny,nz);
STRIP->settex(u,0.5f-v/2);
STRIP->addvtx(nx*radius,ny*radius,nz*radius);
}
}
}
/**********************************************************************
* Given the position of the robot and the distance of a particular
* sonar the collision line representing this reading is given back.
**********************************************************************/
LineSeg sonar_to_lineseg( Point rpos, float rrot,
int sonar_no, float dist)
{
Point sonar_pos, sonar_left_edge, sonar_right_edge;
LineSeg line;
float sonar_rot, sonar_plain, open_angle, tmp, start_angle;
open_angle = SONAR_OPEN_ANGLE;
/* If the line is too far away we don't consider it. */
if (dist > 0.0 && dist <= MAX_RANGE) {
if ( armState == INSIDE) {
/* If the robot is very fast we don't consider the lines behind it.
* If the current tvel is 100.0 we only use lines starting at 90.0
* degrees. This is only allowed if the arm is inside.
*/
tmp = (float) rwi_base.trans_current_speed * 0.01;
start_angle = DEG_180 - tmp * DEG_90;
if ( SonarAngle[sonar_no] > start_angle &&
SonarAngle[sonar_no] < DEG_360 - start_angle) {
line.pt1.x = F_ERROR;
return(line);
}
}
/* We want to limit the length of the lineseg to MAX_COLLISION_LINE_LENGTH */
if (dist > EPSILON && ACTUAL_MODE->max_collision_line_length > 0.0)
if ((tmp = 0.5 * atan(ACTUAL_MODE->max_collision_line_length / dist))
< open_angle)
open_angle = tmp;
sonar_rot = rrot + SonarAngle[sonar_no];
norm_angle(&sonar_rot);
dist /= fcos(open_angle);
sonar_plain = sonar_rot + DEG_90;
sonar_pos.x = rpos.x + (fcos( sonar_rot) * ROB_RADIUS);
sonar_pos.y = rpos.y + (fsin( sonar_rot) * ROB_RADIUS);
sonar_left_edge.x = sonar_pos.x + (fcos( sonar_plain) * COLLI_SONAR_RADIUS);
sonar_left_edge.y = sonar_pos.y + (fsin( sonar_plain) * COLLI_SONAR_RADIUS);
sonar_right_edge.x = sonar_pos.x - (fcos( sonar_plain) * COLLI_SONAR_RADIUS);
sonar_right_edge.y = sonar_pos.y - (fsin( sonar_plain) * COLLI_SONAR_RADIUS);
line.pt1.x = sonar_left_edge.x + fcos( sonar_rot + open_angle) * dist;
line.pt1.y = sonar_left_edge.y + fsin( sonar_rot + open_angle) * dist;
line.pt2.x = sonar_right_edge.x + fcos( sonar_rot - open_angle) * dist;
line.pt2.y = sonar_right_edge.y + fsin( sonar_rot - open_angle) * dist;
/* We want the point with the smaller x value (if possible) to be pt1 */
if (smaller( line.pt2, line.pt1))
swap(&line);
return(line);
}
else line.pt1.x = F_ERROR;
return(line);
}
//static
void
GradientsBase::solveImage(imageType_t const &rLaplaceImage_p, imageType_t &rSolution_p)
{
int const nRows=rLaplaceImage_p.Height();
int const nCols=rLaplaceImage_p.Width();
int const nChannels=rLaplaceImage_p.NumberOfChannels();
imageType_t::color_space colorSpace=rLaplaceImage_p.ColorSpace();
#ifdef USE_FFTW
// adapted from http://www.umiacs.umd.edu/~aagrawal/software.html,
AssertColImage(rLaplaceImage_p);
// just in case we accidentally change this, because code below believes in double...
Assert(typeid(realType_t)==typeid(double));
// check assumption of row major format
Assert(rLaplaceImage_p.PixelAddress(0,0)+1==rLaplaceImage_p.PixelAddress(1,0));
rSolution_p.AllocateData(nCols,nRows,nChannels,colorSpace);
rSolution_p.ResetSelections();
rSolution_p.Black();
#ifdef USE_THREADS
// threaded version
int const nElements=nRows*nCols;
int const nThreads=Thread::NumberOfThreads(nElements);
if(fftw_init_threads()==0){
throw Error("Problem initilizing threads");
}
fftw_plan_with_nthreads(nThreads);
#endif
for(int chan=0;chan<nChannels;++chan){
TimeMessage startSolver(String("FFTW Solver, Channel ")+String(chan));
// FIXME see if fttw_allocate gives us something...
imageType_t fcos(nCols,nRows);
#if 0
// During experiment, the higher optimization did not give us anything except for an additional delay. May change later.
fftw_plan pForward= fftw_plan_r2r_2d(nRows, nCols, const_cast<double *>(rLaplaceImage_p.PixelData(chan)), fcos.PixelData(), FFTW_REDFT10, FFTW_REDFT10, FFTW_MEASURE);
fftw_plan pInverse = fftw_plan_r2r_2d(nRows, nCols, fcos.PixelData(), rSolution_p.PixelData(chan), FFTW_REDFT01, FFTW_REDFT01, FFTW_ESTIMATE);
#else
fftw_plan pForward= fftw_plan_r2r_2d(nRows, nCols, const_cast<double *>(rLaplaceImage_p.PixelData(chan)), fcos.PixelData(), FFTW_REDFT10, FFTW_REDFT10, FFTW_MEASURE);
fftw_plan pInverse = fftw_plan_r2r_2d(nRows, nCols, fcos.PixelData(), rSolution_p.PixelData(chan), FFTW_REDFT01, FFTW_REDFT01, FFTW_ESTIMATE);
#endif
// find DCT
fftw_execute(pForward);
realType_t const pi=pcl::Pi();
for(int row = 0 ; row < nRows; ++row){
for(int col = 0 ; col < nCols; ++col){
fcos.Pixel(col,row) /= 2*cos(pi*col/( (double) nCols)) - 2 + 2*cos(pi*row/((double) nRows)) - 2;
}
}
fcos.Pixel(0,0)=0.0;
// Inverse DCT
fftw_execute(pInverse);
fftw_destroy_plan(pForward);
fftw_destroy_plan(pInverse);
}
#endif
#ifdef USE_PIFFT
// use PI FFT based solver by Carlos Milovic F.
rLaplaceImage_p.ResetSelections();
rSolution_p.AllocateData(nCols,nRows,nChannels,colorSpace);
rSolution_p.ResetSelections();
// current solver handles only one channel per run.
for(int chan=0;chan<nChannels;++chan){
TimeMessage startSolver(String("PIFFT Solver, Channel ")+String(chan));
imageType_t tmpImage(nCols,nRows);
rLaplaceImage_p.SelectChannel(chan);
tmpImage.Assign(rLaplaceImage_p);
__SolvePoisson(tmpImage);
rSolution_p.SelectChannel(chan);
rSolution_p.Mov(tmpImage);
}
#endif
}
for( i=0; i<3; i++)
{ // do not calculate the 4th element (will be normalized)
k = 0;
for( j=0; j<3; j++)
{ // simplified pi[3] is always 1
k += pi[ j] * m[ i][ j];
}
po[ i] = (k + m[i][3])>>8;
}
} //matrix prod
void initWorld( short alpha, short beta, short gamma, int x0, int y0, int z0)
{
m[0][0] = (+fcos( gamma) * fcos( beta)) >>8;
m[0][1] = (-fsin( gamma) * fcos( beta)) >>8;
m[0][2] = fsin( beta);
m[0][3] = x0 << 8;
m[1][0] = ((fcos( gamma) * fsin( beta) * fsin( alpha)) >>16) + ((fsin( gamma)* fcos(alpha)) >>8);
m[1][1] = ((fcos( gamma) * fcos( alpha)) >>8) - ((fsin( gamma) * fsin(beta) * fsin(alpha)) >>16);
m[1][2] = (-fcos( beta) * fsin( alpha)) >>8;
m[1][3] = y0 << 8;
m[2][0] = ((fsin( gamma) * fsin( alpha)) >>8) - ((fcos(gamma) * fsin( beta) * fcos(alpha)) >>16);
m[2][1] = ((fsin( gamma) * fsin( beta) * fcos( alpha)) >>16) + ((fsin( alpha) * fcos( gamma)) >>8);
m[2][2] = (fcos( beta) * fcos( alpha)) >>8;
m[2][3] = z0 << 8;
m[3][0] = 0;
m[3][1] = 0;
m[3][2] = 0;
m[3][3] = 1 << 8;
void
render_torus (float outer, float inner, int front)
{
int major;
const int minor_steps = 15, major_steps = 30;
float major_ang = 0.0, major_incr = 2.0 * M_PI / major_steps;
float first_row[minor_steps + 1][2][3], prev_row[minor_steps + 1][2][3];
/* We need this to calculate shininess properly. */
grab_transform ();
for (major = 0; major <= major_steps; major++, major_ang += major_incr)
{
int minor;
float fsin_major = fsin (major_ang);
float fcos_major = fcos (major_ang);
float maj_x = fcos_major * outer;
float maj_y = fsin_major * outer;
float minor_ang = 0.0, minor_incr = 2.0 * M_PI / minor_steps;
float first[2][3] = { { 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0 } };
float last[2][3] = { { 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0 } };
for (minor = 0; minor <= minor_steps; minor++, minor_ang += minor_incr)
{
float fsin_minor = fsin (minor_ang);
float fcos_minor = fcos (minor_ang);
float min[2][3], orig_min_x;
min[1][0] = fcos_major * fsin_minor;
min[1][1] = fsin_major * fsin_minor;
min[1][2] = fcos_minor;
/* 2D rotation is just:
[ cos T -sin T ] (x)
[ sin T cos T ] (y).
y is zero, so the corresponding terms disappear. */
orig_min_x = fsin_minor * inner;
min[0][0] = fcos_major * orig_min_x + maj_x;
min[0][1] = fsin_major * orig_min_x + maj_y;
min[0][2] = fcos_minor * inner;
if (major == 0)
{
memcpy (&prev_row[minor][0][0], &min[0][0], 6 * sizeof (float));
memcpy (&first_row[minor][0][0], &min[0][0], 6 * sizeof (float));
continue;
}
if (minor == 0)
memcpy (&first[0], &min[0], 6 * sizeof (float));
else
{
int minor_wrapped = (minor == minor_steps) ? 0 : minor;
float texcoords[12];
GLfloat *vptr[4];
int vnum = 0;
/* Polygon is formed from:
prev_row[minor-1] ,-> last_xyz
v / v
prev_row[minor] -' min_xyz. */
//glColor4ub (255, 255, 255, 0);
parabolic_texcoords (&texcoords[0],
prev_row[minor - 1][0],
prev_row[minor - 1][1],
front);
vptr[vnum++] = (GLfloat *) &prev_row[minor - 1][0];
//glColor4ub (255, 0, 0, 0);
parabolic_texcoords (&texcoords[3],
prev_row[minor][0],
prev_row[minor][1],
front);
vptr[vnum++] = (GLfloat *) &prev_row[minor][0];
if (major == major_steps)
{
//glColor4ub (255, 255, 0, 0);
parabolic_texcoords (&texcoords[6],
first_row[minor - 1][0],
first_row[minor - 1][1],
front);
vptr[vnum++] = (GLfloat *) &first_row[minor - 1][0];
//glColor4ub (0, 255, 0, 0);
parabolic_texcoords (&texcoords[9],
first_row[minor_wrapped][0],
first_row[minor_wrapped][1],
front);
vptr[vnum++] = (GLfloat *) &first_row[minor_wrapped][0];
}
else
{
//glColor4ub (255, 255, 0, 0);
parabolic_texcoords (&texcoords[6], last[0], last[1], front);
vptr[vnum++] = &last[0][0];
//glColor4ub (0, 255, 0, 0);
if (minor == minor_steps)
{
parabolic_texcoords (&texcoords[9], first[0], first[1],
front);
vptr[vnum++] = &first[0][0];
}
else
{
parabolic_texcoords (&texcoords[9], min[0], min[1],
front);
vptr[vnum++] = &min[0][0];
}
}
if ((front && (texcoords[2] > 0 || texcoords[5] > 0
|| texcoords[8] > 0 || texcoords[11] > 0))
|| (!front && (texcoords[2] < 0 || texcoords[5] < 0
|| texcoords[8] < 0 || texcoords[11] < 0)))
{
glBegin (GL_TRIANGLE_STRIP);
glTexCoord2f (texcoords[0], texcoords[1]);
glVertex3fv (vptr[0]);
glTexCoord2f (texcoords[3], texcoords[4]);
glVertex3fv (vptr[1]);
glTexCoord2f (texcoords[6], texcoords[7]);
glVertex3fv (vptr[2]);
glTexCoord2f (texcoords[9], texcoords[10]);
glVertex3fv (vptr[3]);
glEnd ();
}
memcpy (&prev_row[minor - 1][0][0], &last[0][0],
6 * sizeof (float));
}
memcpy (&last[0][0], &min[0][0], 6 * sizeof (float));
}
memcpy (&prev_row[minor_steps][0][0], &prev_row[0][0],
6 * sizeof (float));
}
}
void test_mode_7 ()
{
MODE_7_PARAMS params;
BITMAP *tile, *sprite, *buffer;
PALETTE pal;
int quit = FALSE;
fixed angle = itofix (0);
fixed x = 0, y = 0;
fixed dx = 0, dy = 0;
fixed speed = 0;
int i, j, r2;
params.space_z = itofix (50);
params.scale_x = ftofix (200.0);
params.scale_y = ftofix (200.0);
params.obj_scale_x = ftofix (50.0);
params.obj_scale_y = ftofix (50.0);
params.horizon = 20;
// to avoid flicker the program makes use of a double-buffering system
buffer = create_bitmap (screen->w, screen->h);
// create a 64x64 tile bitmap and draw something on it.
tile = create_bitmap (64, 64);
for (i = 0; i < 32; i++)
{
for (j = i; j < 32; j++)
{
putpixel (tile, i, j, i);
putpixel (tile, i, 63-j, i);
putpixel (tile, 63-i, j, i);
putpixel (tile, 63-i, 63-j, i);
putpixel (tile, j, i, i);
putpixel (tile, j, 63-i, i);
putpixel (tile, 63-j, i, i);
putpixel (tile, 63-j, 63-i, i);
}
}
text_mode (-1);
// Create another bitmap and draw something to it.
// This bitmap contains the object.
sprite = create_bitmap (64, 64);
clear (sprite);
for (i = 0; i < 64; i++)
{
for (j = 0; j < 64; j++)
{
r2 = (32 - i) * (32 - i) + (32 - j) * (32 - j);
if (r2 < 30 * 30)
{
r2 = (24 - i) * (24 - i) + (24 - j) * (24 - j);
putpixel (sprite, i, j, 127 - fixtoi(fsqrt(itofix(r2))));
}
}
}
// create a palette
// colors for the tiles
for (i = 0; i < 64; i++)
{
pal[i].r = i;
pal[i].g = i;
pal[i].b = 0;
}
// colors for the object
for (i = 0; i < 32; i++)
{
pal[i+64].r = 0;
pal[i+64].g = 0;
pal[i+64].b = 2 * i;
pal[i+96].r = 2 * i;
pal[i+96].g = 2 * i;
pal[i+96].b = 63;
}
set_palette (pal);
while (!quit)
{
// act on keyboard input
if (key[KEY_ESC]) quit = TRUE;
if (key[KEY_UP] && speed < itofix (5))
speed += ftofix (0.1);
if (key[KEY_DOWN] && speed > itofix (-5))
speed -= ftofix (0.1);
if (key[KEY_LEFT])
angle = (angle - itofix (3)) & 0xFFFFFF;
if (key[KEY_RIGHT])
angle = (angle + itofix (3)) & 0xFFFFFF;
if (key[KEY_Z])
params.space_z += itofix(5);
if (key[KEY_X])
params.space_z -= itofix(5);
if (key[KEY_Q])
params.scale_x = fmul (params.scale_x, ftofix (1.5));
if (key[KEY_W])
params.scale_x = fdiv (params.scale_x, ftofix (1.5));
if (key[KEY_E])
params.scale_y = fmul (params.scale_y, ftofix (1.5));
if (key[KEY_R])
params.scale_y = fdiv (params.scale_y, ftofix (1.5));
if (key[KEY_H])
params.horizon++;
if (key[KEY_J])
params.horizon--;
dx = fmul (speed, fcos (angle));
dy = fmul (speed, fsin (angle));
x += dx;
y += dy;
mode_7 (buffer, tile, angle, x, y, params);
draw_object (buffer, sprite, angle, x, y, params);
vsync();
blit (buffer, screen, 0, 0, 0, 0, SCREEN_W, SCREEN_H);
}
destroy_bitmap (tile);
destroy_bitmap (sprite);
destroy_bitmap (buffer);
}
double fcos_error(x) {
return fabs(fabs(fcos(x)) - fabs(cos(x)));
}
static BOOL act(int p,int q)
{ /* act on selected key */
int k,n,c;
aprint(PRESSED,4+5*p,6+3*q,keys[q][p]);
switch(p+7*q)
{
case 0: if (degrees) fmul(x,radeg,x);
if (hyp) fsinh(x,x);
else fsin(x,x);
newx=TRUE;
break;
case 1: if (degrees) fmul(x,radeg,x);
if (hyp) fcosh(x,x);
else fcos(x,x);
newx=TRUE;
break;
case 2: if (degrees) fmul(x,radeg,x);
if (hyp) ftanh(x,x);
else ftan(x,x);
newx=TRUE;
break;
case 3: if (lgbase>0)
{
n=size(x);
if (abs(n)<MR_TOOBIG)
{
convert(lgbase,x);
if (n<0) frecip(x,x);
fpower(x,abs(n),x);
newx=TRUE;
break;
}
if (lgbase==2) fmul(x,loge2,x);
if (lgbase==10) fmul(x,loge10,x);
}
fexp(x,x);
newx=TRUE;
break;
case 4: mip->RPOINT=!mip->RPOINT;
newx=TRUE;
break;
case 5: clrall();
newx=TRUE;
break;
case 6: return TRUE;
case 7: if (hyp) fasinh(x,x);
else fasin(x,x);
if (degrees) fdiv(x,radeg,x);
newx=TRUE;
break;
case 8: if (hyp) facosh(x,x);
else facos(x,x);
if (degrees) fdiv(x,radeg,x);
newx=TRUE;
break;
case 9: if (hyp) fatanh(x,x);
else fatan(x,x);
if (degrees) fdiv(x,radeg,x);
newx=TRUE;
break;
case 10: flog(x,x);
if (lgbase==2) fdiv(x,loge2,x);
if (lgbase==10) fdiv(x,loge10,x);
newx=TRUE;
break;
case 11: newx=TRUE;
k=3;
forever
{
aprint(INVER,2+stptr[k],2,settings[k][option[k]]);
curser(2+stptr[k],2);
c=arrow(gethit());
if (c==1)
{
if (option[k]==nops[k]) option[k]=0;
else option[k]+=1;
continue;
}
aprint(STATCOL,2+stptr[k],2,settings[k][option[k]]);
if (c==0 || c==2) break;
if (c==4 && k>0) k--;
if (c==3 && k<3) k++;
}
setopts();
break;
case 12: chekit(7);
break;
case 13: result=FALSE;
if (ipt==0) break;
ipt--;
mybuff[ipt]='\0';
if (ipt==0) clr();
just(mybuff);
cinstr(x,mybuff);
newx=TRUE;
break;
case 14: if (!next('7')) putchar(BELL);
break;
case 15: if (!next('8')) putchar(BELL);
break;
case 16: if (!next('9')) putchar(BELL);
break;
case 17: chekit(6);
break;
case 18: chekit(5);
break;
case 19: chekit(4);
break;
case 20: copy(m,x);
newx=TRUE;
break;
case 21: if (!next('4')) putchar(BELL);
break;
case 22: if (!next('5')) putchar(BELL);
break;
case 23: if (!next('6')) putchar(BELL);
break;
case 24: fmul(x,x,x);
newx=TRUE;
break;
case 25: froot(x,2,x);
newx=TRUE;
break;
case 26: chekit(3);
break;
case 27: brkt=0;
chekit(0);
flag=OFF;
fadd(m,x,m);
newx=TRUE;
break;
case 28: if (!next('1')) putchar(BELL);
break;
case 29: if (!next('2')) putchar(BELL);
break;
case 30: if (!next('3')) putchar(BELL);
break;
case 31: frecip(x,x);
newx=TRUE;
break;
case 32: fpi(x);
newx=TRUE;
break;
case 33: chekit(2);
break;
case 34: negify(x,x);
newx=TRUE;
break;
case 35: if (!next('0')) putchar(BELL);
break;
case 36: if (!next('/')) putchar(BELL);
break;
case 37: if (!next('.')) putchar(BELL);
break;
case 38: if (ipt>0)
{
putchar(BELL);
result=FALSE;
}
else
{
zero(x);
brkt+=1;
newx=TRUE;
}
break;
case 39: if (brkt>0)
{
chekit(0);
brkt-=1;
}
else
{
putchar(BELL);
result=FALSE;
}
break;
case 40: chekit(1);
break;
case 41: brkt=0;
equals(0);
flag=OFF;
break;
}
return FALSE;
}
// Renders a slide to offscreen buffer. Returns a rect of the rendered area.
// alpha=256 means normal, alpha=0 is fully black, alpha=128 half transparent
// col1 and col2 limit the column for rendering.
QRect PictureFlowPrivate::renderSlide(const SlideInfo &slide, int alpha,
int col1, int col2)
{
QImage* src = surface(slide.slideIndex);
if(!src)
return QRect();
QRect rect(0, 0, 0, 0);
#ifdef PICTUREFLOW_BILINEAR_FILTER
int sw = src->height() / BILINEAR_STRETCH_HOR;
int sh = src->width() / BILINEAR_STRETCH_VER;
#else
int sw = src->height();
int sh = src->width();
#endif
int h = buffer.height();
int w = buffer.width();
if(col1 > col2)
{
int c = col2;
col2 = col1;
col1 = c;
}
col1 = (col1 >= 0) ? col1 : 0;
col2 = (col2 >= 0) ? col2 : w-1;
col1 = qMin(col1, w-1);
col2 = qMin(col2, w-1);
int distance = h * 100 / zoom;
PFreal sdx = fcos(slide.angle);
PFreal sdy = fsin(slide.angle);
PFreal xs = slide.cx - slideWidth * sdx/2;
PFreal ys = slide.cy - slideWidth * sdy/2;
PFreal dist = distance * PFREAL_ONE;
int xi = qMax((PFreal)0, ((w*PFREAL_ONE/2) + fdiv(xs*h, dist+ys)) >> PFREAL_SHIFT);
if(xi >= w)
return rect;
bool flag = false;
rect.setLeft(xi);
for(int x = qMax(xi, col1); x <= col2; x++)
{
PFreal hity = 0;
PFreal fk = rays[x];
if(sdy)
{
fk = fk - fdiv(sdx,sdy);
hity = -fdiv((rays[x]*distance - slide.cx + slide.cy*sdx/sdy), fk);
}
dist = distance*PFREAL_ONE + hity;
if(dist < 0)
continue;
PFreal hitx = fmul(dist, rays[x]);
PFreal hitdist = fdiv(hitx - slide.cx, sdx);
#ifdef PICTUREFLOW_BILINEAR_FILTER
int column = sw*BILINEAR_STRETCH_HOR/2 + (hitdist*BILINEAR_STRETCH_HOR >> PFREAL_SHIFT);
if(column >= sw*BILINEAR_STRETCH_HOR)
break;
#else
int column = sw/2 + (hitdist >> PFREAL_SHIFT);
if(column >= sw)
break;
#endif
if(column < 0)
continue;
rect.setRight(x);
if(!flag)
rect.setLeft(x);
flag = true;
int y1 = h/2;
int y2 = y1+ 1;
QRgb* pixel1 = (QRgb*)(buffer.scanLine(y1)) + x;
QRgb* pixel2 = (QRgb*)(buffer.scanLine(y2)) + x;
QRgb pixelstep = pixel2 - pixel1;
#ifdef PICTUREFLOW_BILINEAR_FILTER
int center = (sh*BILINEAR_STRETCH_VER/2);
int dy = dist*BILINEAR_STRETCH_VER / h;
#else
int center = (sh/2);
int dy = dist / h;
#endif
int p1 = center*PFREAL_ONE - dy/2;
int p2 = center*PFREAL_ONE + dy/2;
const QRgb *ptr = (const QRgb*)(src->scanLine(column));
if(alpha == 256)
while((y1 >= 0) && (y2 < h) && (p1 >= 0))
{
*pixel1 = ptr[p1 >> PFREAL_SHIFT];
*pixel2 = ptr[p2 >> PFREAL_SHIFT];
p1 -= dy;
p2 += dy;
y1--;
y2++;
pixel1 -= pixelstep;
pixel2 += pixelstep;
}
else
while((y1 >= 0) && (y2 < h) && (p1 >= 0))
{
QRgb c1 = ptr[p1 >> PFREAL_SHIFT];
QRgb c2 = ptr[p2 >> PFREAL_SHIFT];
int r1 = qRed(c1) * alpha/256;
int g1 = qGreen(c1) * alpha/256;
int b1 = qBlue(c1) * alpha/256;
int r2 = qRed(c2) * alpha/256;
int g2 = qGreen(c2) * alpha/256;
int b2 = qBlue(c2) * alpha/256;
*pixel1 = qRgb(r1, g1, b1);
*pixel2 = qRgb(r2, g2, b2);
p1 -= dy;
p2 += dy;
y1--;
y2++;
pixel1 -= pixelstep;
pixel2 += pixelstep;
}
}
