bool PhysicalAABox::intersectCircle(float radius, LineSegment displacement, sf::Vector2f& centerAfterCollision, sf::Vector2f& intersectionPoint, sf::Vector2f& intersectionNormal) const {
// Fast solution that doesn't work well with corners..
sf::Vector2f direction = displacement.end - displacement.start;
sf::Vector2f origin = origin_ - sf::Vector2f(radius, radius);
float width = width_ + radius + radius;
float height = height_ + radius + radius;
float p[4] = { -direction.x, direction.x, -direction.y, direction.y };
float q[4] = { (displacement.start.x - origin.x), (origin.x + width - displacement.start.x), (displacement.start.y - origin.y), (origin.y + height - displacement.start.y) };
float u1{MIN_FLOAT};
float u2{MAX_FLOAT};
for (int i = 0; i < 4; i++) {
if (p[i] == 0.0f) {
if (q[i] < 0.0f)
return false;
}
else {
float t = q[i] / p[i];
if (p[i] < 0.0f && u1 < t)
u1 = t;
else if (p[i] > 0.0f && u2 > t)
u2 = t;
}
}
if (u1 > u2 || u1 > 1.0f || u1 < 0.0f)
return false;
centerAfterCollision = displacement.start + u1*direction;
if ( equalf(centerAfterCollision.x, origin.x) )
intersectionNormal = sf::Vector2f(-1, 0);
else if ( equalf(centerAfterCollision.x, origin.x + width) )
intersectionNormal = sf::Vector2f(1, 0);
else if ( equalf(centerAfterCollision.y, origin.y) )
intersectionNormal = sf::Vector2f(0, -1);
else if ( equalf(centerAfterCollision.y, origin.y + height) )
intersectionNormal = sf::Vector2f(0, 1);
intersectionPoint = centerAfterCollision - radius*intersectionNormal;
centerAfterCollision = displacement.start + (u1-0.1f)*direction;
return true;
}
bool PhysicalAABox::intersectLineSegment(LineSegment lineSegment, sf::Vector2f& intersectionPoint, sf::Vector2f& intersectionNormal) const {
sf::Vector2f direction = lineSegment.end - lineSegment.start;
float p[4] = { -direction.x, direction.x, -direction.y, direction.y };
float q[4] = { (lineSegment.start.x - origin_.x), (origin_.x + width_ - lineSegment.start.x), (lineSegment.start.y - origin_.y), (origin_.y + height_ - lineSegment.start.y) };
float u1{MIN_FLOAT};
float u2{MAX_FLOAT};
for (int i = 0; i < 4; i++) {
if (p[i] == 0.0f) {
if (q[i] < 0.0f)
return false;
}
else {
float t = q[i] / p[i];
if (p[i] < 0.0f && u1 < t)
u1 = t;
else if (p[i] > 0.0f && u2 > t)
u2 = t;
}
}
if (u1 > u2 || u1 > 1.0f || u1 < 0.0f)
return false;
intersectionPoint = lineSegment.start + u1*direction;
if ( equalf(intersectionPoint.x, origin_.x) )
intersectionNormal = sf::Vector2f(-1, 0);
else if ( equalf(intersectionPoint.x, origin_.x + width_) )
intersectionNormal = sf::Vector2f(1, 0);
else if ( equalf(intersectionPoint.y, origin_.y) )
intersectionNormal = sf::Vector2f(0, -1);
else if ( equalf(intersectionPoint.y, origin_.y + height_) )
intersectionNormal = sf::Vector2f(0, 1);
return true;
}
static int
binop(void)
{
const char *opnd1, *opnd2;
struct t_op const *op;
opnd1 = *t_wp;
(void) t_lex(*++t_wp);
op = t_wp_op;
if ((opnd2 = *++t_wp) == NULL)
return syntax(op->op_text, "argument expected");
switch (op->op_num) {
case STREQ:
return strcmp(opnd1, opnd2) == 0;
case STRNE:
return strcmp(opnd1, opnd2) != 0;
case STRLT:
return strcmp(opnd1, opnd2) < 0;
case STRGT:
return strcmp(opnd1, opnd2) > 0;
case INTEQ:
return getn(opnd1) == getn(opnd2);
case INTNE:
return getn(opnd1) != getn(opnd2);
case INTGE:
return getn(opnd1) >= getn(opnd2);
case INTGT:
return getn(opnd1) > getn(opnd2);
case INTLE:
return getn(opnd1) <= getn(opnd2);
case INTLT:
return getn(opnd1) < getn(opnd2);
case FILNT:
return newerf(opnd1, opnd2);
case FILOT:
return olderf(opnd1, opnd2);
case FILEQ:
return equalf(opnd1, opnd2);
default:
abort();
/* NOTREACHED */
#ifdef _MSC_VER
return -42;
#endif
}
}
static int
binop(shinstance *psh)
{
const char *opnd1, *opnd2;
struct t_op const *op;
opnd1 = *psh->t_wp;
(void) t_lex(psh, *++psh->t_wp);
op = psh->t_wp_op;
if ((opnd2 = *++psh->t_wp) == NULL)
syntax(psh, op->op_text, "argument expected");
switch (op->op_num) {
case STREQ:
return strcmp(opnd1, opnd2) == 0;
case STRNE:
return strcmp(opnd1, opnd2) != 0;
case STRLT:
return strcmp(opnd1, opnd2) < 0;
case STRGT:
return strcmp(opnd1, opnd2) > 0;
case INTEQ:
return getn(psh, opnd1) == getn(psh, opnd2);
case INTNE:
return getn(psh, opnd1) != getn(psh, opnd2);
case INTGE:
return getn(psh, opnd1) >= getn(psh, opnd2);
case INTGT:
return getn(psh, opnd1) > getn(psh, opnd2);
case INTLE:
return getn(psh, opnd1) <= getn(psh, opnd2);
case INTLT:
return getn(psh, opnd1) < getn(psh, opnd2);
case FILNT:
return newerf(psh, opnd1, opnd2);
case FILOT:
return olderf(psh, opnd1, opnd2);
case FILEQ:
return equalf(psh, opnd1, opnd2);
default:
sh_abort(psh);
/* NOTREACHED */
return -1;
}
}
static int
binop(void)
{
const char *opnd1, *opnd2;
struct t_op const *op;
opnd1 = *t_wp;
(void) t_lex(nargc > 0 ? (--nargc, *++t_wp) : NULL);
op = t_wp_op;
if ((opnd2 = nargc > 0 ? (--nargc, *++t_wp) : NULL) == NULL)
syntax(op->op_text, "argument expected");
switch (op->op_num) {
case STREQ:
return strcmp(opnd1, opnd2) == 0;
case STRNE:
return strcmp(opnd1, opnd2) != 0;
case STRLT:
return strcmp(opnd1, opnd2) < 0;
case STRGT:
return strcmp(opnd1, opnd2) > 0;
case INTEQ:
return intcmp(opnd1, opnd2) == 0;
case INTNE:
return intcmp(opnd1, opnd2) != 0;
case INTGE:
return intcmp(opnd1, opnd2) >= 0;
case INTGT:
return intcmp(opnd1, opnd2) > 0;
case INTLE:
return intcmp(opnd1, opnd2) <= 0;
case INTLT:
return intcmp(opnd1, opnd2) < 0;
case FILNT:
return newerf (opnd1, opnd2);
case FILOT:
return olderf (opnd1, opnd2);
case FILEQ:
return equalf (opnd1, opnd2);
default:
abort();
/* NOTREACHED */
}
}
static int
binop(enum token n)
{
const char *opnd1, *op, *opnd2;
opnd1 = *t_wp;
op = nargc > 0 ? (--nargc, *++t_wp) : NULL;
if ((opnd2 = nargc > 0 ? (--nargc, *++t_wp) : NULL) == NULL)
syntax(op, "argument expected");
switch (n) {
case STREQ:
return strcmp(opnd1, opnd2) == 0;
case STRNE:
return strcmp(opnd1, opnd2) != 0;
case STRLT:
return strcmp(opnd1, opnd2) < 0;
case STRGT:
return strcmp(opnd1, opnd2) > 0;
case INTEQ:
return intcmp(opnd1, opnd2) == 0;
case INTNE:
return intcmp(opnd1, opnd2) != 0;
case INTGE:
return intcmp(opnd1, opnd2) >= 0;
case INTGT:
return intcmp(opnd1, opnd2) > 0;
case INTLE:
return intcmp(opnd1, opnd2) <= 0;
case INTLT:
return intcmp(opnd1, opnd2) < 0;
case FILNT:
return newerf (opnd1, opnd2);
case FILOT:
return olderf (opnd1, opnd2);
case FILEQ:
return equalf (opnd1, opnd2);
default:
abort();
/* NOTREACHED */
}
}
bool PhysicalPolygon::lineIntersect(Line line, LineSegment lineSegment, float& t, float& u) const {
sf::Vector2f r = line.direction;
sf::Vector2f s = lineSegment.end - lineSegment.start;
sf::Vector2f CmP = lineSegment.start - line.origin;
float rxs = cross(r, s);
if (equalf(rxs, 0.0f))
return false;
float CmPxr = cross(CmP, r);
float CmPxs = cross(CmP, s);
float rxsr = 1.0f / rxs;
t = CmPxs * rxsr;
u = CmPxr * rxsr;
return true;
}
static int binop(void)
{
const char *opnd1, *opnd2;
struct t_op const *op;
opnd1 = *t_wp;
(void) t_lex(*++t_wp);
op = t_wp_op;
if ((opnd2 = *++t_wp) == (char *) 0)
syntax(op->op_text, "argument expected");
switch (op->op_num) {
case STREQ:
return strcmp(opnd1, opnd2) == 0;
case STRNE:
return strcmp(opnd1, opnd2) != 0;
case STRLT:
return strcmp(opnd1, opnd2) < 0;
case STRGT:
return strcmp(opnd1, opnd2) > 0;
case INTEQ:
return getn(opnd1) == getn(opnd2);
case INTNE:
return getn(opnd1) != getn(opnd2);
case INTGE:
return getn(opnd1) >= getn(opnd2);
case INTGT:
return getn(opnd1) > getn(opnd2);
case INTLE:
return getn(opnd1) <= getn(opnd2);
case INTLT:
return getn(opnd1) < getn(opnd2);
case FILNT:
return newerf(opnd1, opnd2);
case FILOT:
return olderf(opnd1, opnd2);
case FILEQ:
return equalf(opnd1, opnd2);
}
/* NOTREACHED */
return 1;
}
// 平顶三角形，朝下
void Renderer::FlatTriangleDown(const Vector3 &v0, const Vector3 &v1, const Vector3 &v2, uint32 color)
{
assert(equalf(v0.y, v1.y));
assert(v0.y < v2.y);
float dx_left = (v2.x - v0.x) / (v2.y - v0.y);
float dx_right = (v2.x - v1.x) / (v2.y - v0.y);
float sx = v0.x;
float ex = v1.x;
for (int i = round(v0.y); i < round(v2.y); ++i)
{
for (int j = round(sx); j < round(ex); ++j)
{
DrawPixel(j, i, color);
}
sx += dx_left;
ex += dx_right;
}
}
void
step()
{
// printf("\nSTEP %d\n", PROGRAM->frame);
draw_set(COLOR_BLACK);
Entity *entity;
memi i;
player_step(GAME->player);
//
// Update entities
//
entity = GAME->entities;
for (i = 0;
i != GAME->entities_count;
++i, ++entity)
{
if (!equalf(entity->vx, 0, 0.1) || !equalf(entity->vy, 0, WORLD_COLLISION_EPSILON))
{
world_test_move(&GAME->world,
entity->collider,
entity->vx, entity->vy,
CollisionTestMask_All,
&collision,
on_collision);
if (!equalf(collision.end_x, entity->collider->x, WORLD_COLLISION_EPSILON) ||
!equalf(collision.end_y, entity->collider->y, WORLD_COLLISION_EPSILON))
{
world_move(&GAME->world,
entity->collider,
collision.end_x,
collision.end_y);
}
}
}
player_camera_step(GAME->player, &GAME->camera);
PROGRAM->tx = -GAME->camera.x + (SCREEN_WIDTH >> 1);
PROGRAM->ty = -GAME->camera.y + (SCREEN_HEIGHT >> 1);
//
// Update animations
//
if ((PROGRAM->frame % 3) == 0)
{
Animation *ani = GAME->animations;
for (i = 0;
i != GAME->animations_count;
++i, ++ani)
{
ani->sprite++;
if (ani->sprite == ani->end) {
ani->sprite = ani->begin;
}
}
}
//
// Draw
//
tilemap_draw(res_level_0_layer1, res_image_set_main, 0, 0);
Collider *collider;
entity = GAME->entities;
for (i = 0;
i != GAME->entities_count;
++i, ++entity)
{
collider = entity->collider;
rect_draw(v4i_make_size(roundf(collider->x - collider->w),
roundf(collider->y - collider->h),
collider->w * 2,
collider->h * 2), COLOR_SHADE_3);
// image_set_draw(collider->x + entity->sprite_ox, collider->y + entity->sprite_oy,
// entity->animation->set,
// entity->animation->sprite,
// entity->animation->sprite_mode, 0);
}
//
// Debug
//
debug_world_colliders_count(&GAME->world);
PROGRAM->tx = 0;
PROGRAM->ty = 0;
debug_buttons(4, SCREEN_HEIGHT - res_icons->ch - 4);
debug_fps(SCREEN_WIDTH - (7 * res_font->cw) - 4,
SCREEN_HEIGHT - res_font->ch - 4,
PROGRAM->time_step);
debug_world_bucket_stats(&GAME->world, 4, 4);
debug_world_bucket_cells(&GAME->world, SCREEN_WIDTH - 4 - 16, 4);
}
void Renderer::FlatTriangle(const Triangle *tri, uint32 color)
{
Vector3 v0 = tri->p[0].GetVector3();
Vector3 v1 = tri->p[1].GetVector3();
Vector3 v2 = tri->p[2].GetVector3();
if (equalf(v0.x, v1.x) && equalf(v1.x, v2.x))
return;
if (equalf(v0.y, v1.y) && equalf(v1.y, v2.y))
return;
// TODO 应该可以利用管线中三角形的性质进行优化d
// 屏幕坐标，y轴朝下
if (v0.y > v1.y)
{
swap(v0, v1);
}
if (v0.y > v2.y)
{
swap(v0, v2);
}
if (v1.y > v2.y)
{
swap(v1, v2);
}
// 平顶三角形
if (equalf(v0.y, v1.y))
{
if (v0.x > v1.x)
swap(v0, v1);
FlatTriangleDown(v0, v1, v2, color);
}
// 平底三角形
else if (equalf(v1.y, v2.y))
{
if (v2.x > v1.x)
swap(v1, v2);
FlatTriangleUp(v0, v1, v2, color);
}
else
{
// 差值计算中点
Vector3 m;
m.y = v1.y;
// m.x = (v0.x - v2.x ) / (v0.y - v2.y) * (m.y - v0.y) + v2.x;
m.x = (v0.x * m.y - v0.x * v2.y - v2.x * m.y + v2.x * v0.y) / (v0.y - v2.y);
// 朝左的三角形
if (v1.x < m.x)
{
FlatTriangleUp(v0, m, v1, color);
FlatTriangleDown(v1, m, v2, color);
}
// 朝右的三角形
else
{
FlatTriangleUp(v0, v1, m, color);
FlatTriangleDown(m, v1, v2, color);
}
}
}
bool Renderer::ClipLine3d(const Vector4 &beg, const Vector4 &end,
const Vector3 &w_min, const Vector3 &w_max, Vector4 *res)
{
assert(res);
static const int BUF_SIZE = 6;
float x0 = beg.x;
float y0 = beg.y;
float z0 = beg.z;
float x_dt = end.x - beg.x;
float y_dt = end.y - beg.y;
float z_dt = end.z - beg.z;
float p[BUF_SIZE] = {0};
float q[BUF_SIZE] = {0};
p[0] = -x_dt;
q[0] = x0 - w_min.x;
p[1] = x_dt;
q[1] = w_max.x - x0;
p[2] = -y_dt;
q[2] = y0 - w_min.y;
p[3] = y_dt;
q[3] = w_max.y - y0;
p[4] = -z_dt;
q[4] = z0 - w_min.z; //
p[5] = z_dt;
q[5] = w_max.z - z0;
float u1 = 0.0f;
float u2 = 1.0f;
for (int i = 0; i < BUF_SIZE; ++i)
{
if (equalf(p[i], 0))
{
if (q[i] < 0)
return false;
continue;
}
float r = q[i] / p[i];
// max of, from out to in.
if (u1 < r && p[i] < 0)
{
u1 = r;
}
// min of, from in to out.
if (u2 > r && p[i] > 0)
{
u2 = r;
}
}
if (u1 > u2)
{
assert(false);
return false;
}
else
{
if (equalf(u1, 0))
{
*res = beg + (u2 * Vector4(x_dt, y_dt, z_dt, 0));
}
else if (equalf(u2, 1))
{
*res = beg + (u1 * Vector4(x_dt, y_dt, z_dt, 0));
}
// 有计算误差
res->x = clamp(res->x, w_min.x, w_max.x);
res->y = clamp(res->y, w_min.y, w_max.y);
return true;
}
assert(false);
return false;
}
/**
* @brief update
* @param imgOut
* @param imgIn
*/
void update(Image *imgOut, Image *imgIn)
{
imgOut->assign(imgIn);
#pragma omp parallel for
for(int i = 0; i < imgIn->height; i++) {
for(int j = 0; j < imgIn->width; j++) {
float *src = (*imgIn)(j, i);
float *n0 = (*imgIn)(j + 1, i);
float *n1 = (*imgIn)(j - 1, i);
float *n2 = (*imgIn)(j, i + 1);
float *n3 = (*imgIn)(j, i - 1);
int ind = i * imgIn->width + j;
float *out = (*imgOut)(j, i);
for(int k = 0; k < imgIn->channels; k++) {
if(equalf(src[k], value)) {
int div = 0;
out[k] = 0.0f;
if(!equalf(n0[k], value)) {
out[k] += n0[k];
div++;
}
if(!equalf(n1[k], value)) {
out[k] += n1[k];
div++;
}
if(!equalf(n2[k], value)) {
out[k] += n2[k];
div++;
}
if(!equalf(n3[k], value)) {
out[k] += n3[k];
div++;
}
if(div > 0) {
out[k] = out[k] / float(div);
mask[ind] = false;
}
} else { //Poisson solver
if(maskPoisson[ind]) {
int div = 0;
float tmp = 0.0f;
if(!equalf(n0[k], value)) {
tmp += -src[k] + n0[k];
div++;
}
if(!equalf(n1[k], value)) {
tmp += -src[k] + n1[k];
div++;
}
if(!equalf(n2[k], value)) {
tmp += -src[k] + n2[k];
div++;
}
if(!equalf(n3[k], value)) {
tmp += -src[k] + n3[k];
div++;
}
out[k] = src[k] + tmp / float(div);
}
}
}
}
}
}
Vector3& operator /=(Vector3& v, float k)
{
assert(!equalf(k, 0) && "Division by zero error.");
v = v / k;
return v;
}
Vector3 normalize(const Vector3& v)
{
assert(!equalf(norm(v), 0) && "Vector is zero.");
return v / norm(v);
}
// File Format:
// rowIndex angle B hits colInd[0] colInd[1] ... colInd[hits-1] endl
// startAngle,angleSpace in degrees
void SimulatePct2d(neo MyNeo,int histories,int yparts,int xparts,
float boxLength,float boxWidth,int totalProjAngles,
float startAngle,float angleSpace,int pathOption,
int phantomOption,string MatrixFilename,
string NeoXtrueFilename)
{
int sliceIndex=0; // 2d sim has only 1 slice
/***** FOLLOWING SECTION IS FOR WRITING TRUE X VECTOR TO FILE*******/
fstream outfileX; // Variable used to write the true X vector to file
outfileX.open(NeoXtrueFilename.c_str(),ios::out); // Open X vector file
if(not outfileX.is_open())
cout << "ERROR 1: SimulatePct2d unable to open NeoXtrueFilename.c_str()";
float* Xtrue = CreateNeoSlice(MyNeo,yparts,xparts,boxLength,
boxWidth,phantomOption);
//Output the true image vector X
for (int i=0; i<yparts*xparts; i++)
outfileX<<Xtrue[i]<<' ';
outfileX<<endl;
outfileX.close(); //Close file for true X vector
/*************** WRITING X VECTOR TO FILE COMPLETE *************/
/************* BEGIN WRITING SYSTEM MATRIX TO FILE *************/
fstream outfile; // Variable used to write System Matrix to file
int version = 0; // Used for determining the version of output,
// in case the program has already been executed.
string fileToOpen;
cout << "\nChecking for latest output version...\n";
do{ // Loop until a nonexisting filename is found.
version++; // Increment # of version for each new file.
//Concatenate the file name with the version # and appropriate suffix.
fileToOpen = MatrixFilename + concatenate("Version", version) + ".txt";
}while(fileExists(fileToOpen)); // Loop until a new filename is made.
outfile.open(fileToOpen.c_str(),ios::out|ios::app); //Open Matrix file
cout << "\nSaving data to "<<fileToOpen << ".\n";
int rowIndex=0;
int rowsPerAngle=ceil(histories/(float)totalProjAngles);
float endAngle=startAngle+totalProjAngles*angleSpace;
cout << "\nBeginning Virtual Proton Projections\n";
//Cycle through each of the angles of the gantry
for (float theta=startAngle; theta<endAngle; theta+=angleSpace)
{
float thetaRad=theta*PI/180.0; //Convert to Radians
if (equalf(thetaRad,0.0) or equalf(thetaRad,PI) or
equalf(thetaRad,PI/2) or equalf(thetaRad,3/2*PI))
thetaRad+=1*PI/180.0; // to prevent division by zero (this will be fixed)
// The # of proton histories per gantry angle is given 'rowsPerAngle'
for (int localRow=0; localRow<rowsPerAngle; localRow++)
{
if (rowIndex<histories) //1 row is dedicated to each history
{
History2d* T=generateProtonPath2d_KnownHull(thetaRad,Xtrue,yparts,
xparts,boxLength,boxWidth,
MyNeo,pathOption,sliceIndex);
if (T->colInd.size()!=0) //If a history was generated?
{
//Identify Matrix Row & Gantry Angle
outfile<<rowIndex<<' '<<thetaRad*180/PI<<' ';
//Identify WEPL Value and the label # of voxels that were intersected
outfile<<T->wepl<<' '<<T->colInd.size()<<' ';
//Cycle through and give #s of each voxel intersected
for (int j=0; j<T->colInd.size(); j++)
outfile<<T->colInd[j]<<' ';
outfile<<endl; //End the current row of the matrix
rowIndex++; //Index Row
}
else
localRow--;
}
}
}
cout << "\nFinished Virtual Proton Projections\n";
outfile.close();
}
const Vector3 operator /(const Vector3& v, float k)
{
assert(!equalf(k, 0) && "Division by zero error.");
const auto invK = 1 / k;
return v * invK;
}
bool operator ==(const Vector3& a, const Vector3& b)
{
return equalf(a.x, b.x) && equalf(a.y, b.y) && equalf(a.z, b.z);
}