static void fcylinder_intersect(cylinder * cyl, ray * ry) {
vector rc, n, O, hit, tmp2, ctmp4;
flt t, s, tin, tout, ln, d, tmp, tmp3;
rc.x = ry->o.x - cyl->ctr.x;
rc.y = ry->o.y - cyl->ctr.y;
rc.z = ry->o.z - cyl->ctr.z;
VCross(&ry->d, &cyl->axis, &n);
VDOT(ln, n, n);
ln=sqrt(ln); /* finish length calculation */
if (ln == 0.0) { /* ray is parallel to the cylinder.. */
return; /* in this case, we want to miss or go through the "hole" */
}
VNorm(&n);
VDOT(d, rc, n);
d = fabs(d);
if (d <= cyl->rad) { /* ray intersects cylinder.. */
VCross(&rc, &cyl->axis, &O);
VDOT(t, O, n);
t = - t / ln;
VCross(&n, &cyl->axis, &O);
VNorm(&O);
VDOT(s, ry->d, O);
s = fabs(sqrt(cyl->rad*cyl->rad - d*d) / s);
tin = t - s;
RAYPNT(hit, (*ry), tin);
ctmp4=cyl->axis;
VNorm(&ctmp4);
tmp2.x = hit.x - cyl->ctr.x;
tmp2.y = hit.y - cyl->ctr.y;
tmp2.z = hit.z - cyl->ctr.z;
VDOT(tmp, tmp2, ctmp4);
VDOT(tmp3, cyl->axis, cyl->axis);
if ((tmp > 0.0) && (tmp < sqrt(tmp3)))
add_intersection(tin, (object *) cyl, ry);
tout = t + s;
RAYPNT(hit, (*ry), tout);
tmp2.x = hit.x - cyl->ctr.x;
tmp2.y = hit.y - cyl->ctr.y;
tmp2.z = hit.z - cyl->ctr.z;
VDOT(tmp, tmp2, ctmp4);
VDOT(tmp3, cyl->axis, cyl->axis);
if ((tmp > 0.0) && (tmp < sqrt(tmp3)))
add_intersection(tout, (object *) cyl, ry);
}
}
void rt_camerasetup(SceneHandle voidscene, apiflt zoom, apiflt aspectratio,
int antialiasing, int raydepth,
vector camcent, vector viewvec, vector upvec) {
scenedef * scene = (scenedef *) voidscene;
vector newupvec;
vector newviewvec;
vector newrightvec;
VCross((vector *) &upvec, &viewvec, &newrightvec);
VNorm(&newrightvec);
VCross((vector *) &viewvec, &newrightvec, &newupvec);
VNorm(&newupvec);
newviewvec=viewvec;
VNorm(&newviewvec);
scene->camzoom=zoom;
scene->aspectratio=aspectratio;
scene->antialiasing=antialiasing;
scene->raydepth=raydepth;
scene->camcent=camcent;
scene->camviewvec=newviewvec;
scene->camrightvec=newrightvec;
scene->camupvec=newupvec;
}
static void cylinder_intersect(const cylinder * cyl, ray * ry) {
vector rc, n, D, O;
flt t, s, tin, tout, ln, d;
rc.x = ry->o.x - cyl->ctr.x;
rc.y = ry->o.y - cyl->ctr.y;
rc.z = ry->o.z - cyl->ctr.z;
VCross(&ry->d, &cyl->axis, &n);
ln=SQRT(n.x*n.x + n.y*n.y + n.z*n.z); /* finish length calculation */
if (ln == 0.0) { /* ray is parallel to the cylinder.. */
VDOT(d, rc, cyl->axis);
D.x = rc.x - d * cyl->axis.x;
D.y = rc.y - d * cyl->axis.y;
D.z = rc.z - d * cyl->axis.z;
VDOT(d, D, D);
d = SQRT(d);
tin = -FHUGE;
tout = FHUGE;
/* if (d <= cyl->rad) then ray is inside cylinder.. else outside */
}
n.x /= ln;
n.y /= ln;
n.z /= ln;
VDOT(d, rc, n);
d = FABS(d);
if (d <= cyl->rad) { /* ray intersects cylinder.. */
VCross(&rc, &cyl->axis, &O);
VDOT(t, O, n);
t = - t / ln;
VCross(&n, &cyl->axis, &O);
ln = SQRT(O.x*O.x + O.y*O.y + O.z*O.z);
O.x /= ln;
O.y /= ln;
O.z /= ln;
VDOT(s, ry->d, O);
s = FABS(SQRT(cyl->rad*cyl->rad - d*d) / s);
tin = t - s;
ry->add_intersection(tin, (object *) cyl, ry);
tout = t + s;
ry->add_intersection(tout, (object *) cyl, ry);
}
}
static FeatureData
Combine_Plane_Plane_Constraints(FeaturePtr c1, FeaturePtr c2)
{
FeatureData result;
Vector cross;
/* The result is inconsistent if the planes are parallel and not
** coincident, a plane if they are parallel and coincident and
** their line of intersection otherwise.
*/
VCross(c1->f_vector, c2->f_vector, cross);
if ( VZero(cross) )
{
/* The planes are parallel. */
if ( Point_On_Plane(c1->f_vector, c1->f_value, c2->f_point) )
return *c1;
result.f_type = inconsistent_feature;
return result;
}
result.f_type = line_feature;
result.f_vector = cross;
/* Project one of the plane points onto the line to get the point. */
if ( ! IsZero(cross.z) )
{
result.f_point.x = ( c2->f_vector.y * c1->f_value -
c1->f_vector.y * c2->f_value ) / cross.z;
result.f_point.y = ( c1->f_vector.x * c2->f_value -
c2->f_vector.x * c1->f_value ) / cross.z;
result.f_point.z = 0;
}
else if ( ! IsZero(cross.y) )
{
result.f_point.x = ( c1->f_vector.z * c2->f_value -
c2->f_vector.z * c1->f_value ) / cross.y;
result.f_point.y = 0;
result.f_point.z = ( c2->f_vector.x * c1->f_value -
c1->f_vector.x * c2->f_value ) / cross.y;
}
else
{
result.f_point.x = 0;
result.f_point.y = ( c2->f_vector.z * c1->f_value -
c1->f_vector.z * c2->f_value ) / cross.x;
result.f_point.z = ( c1->f_vector.y * c2->f_value -
c2->f_vector.y * c1->f_value ) / cross.x;
}
/* Now project a plane point onto the line to get a better point. */
/* The reasons relate to interactive dragging along a line constraint,
** where the distance between the mouse and the origin pt of the line
** affects the accuracy of the motion. */
result.f_point = Closest_Line_Point(cross, result.f_point, c1->f_point);
return result;
}
void DriftCorrection(void)
{
static float MagHeadingX, MagHeadingY;
static float ErrorCourse;
static float Scaled_Omega_P[3], Scaled_Omega_I[3];
static float AccMagnitude, AccWeight;
// Roll and Pitch
AccMagnitude = sqrt(Acc_V[0]*Acc_V[0] + Acc_V[1]*Acc_V[1] + Acc_V[2]*Acc_V[2]);
AccMagnitude = AccMagnitude / (float)GRAVITY; // Scale to gravity.
// dynamic weighting of Accerometer info (reliability filter)
// weight for Accerometer info ( < 0.5G = 0.0, 1G = 1.0 , > 1.5G = 0.0)
AccWeight = constrain(1 - 2 * abs(1 - AccMagnitude), 0, 1);
VCross(&RollPitchError[0], &Acc_V[0], &DCM_M[2][0]); //adjust the ground of reference
VScale(&Omega_P[0], &RollPitchError[0], Kp_RollPitch * AccWeight);
VScale(&Scaled_Omega_I[0], &RollPitchError[0], Ki_RollPitch * AccWeight);
VAdd(Omega_I,Omega_I, Scaled_Omega_I);
// Yaw - drift correction based on compass magnetic heading
MagHeadingX = cos(MagHeading);
MagHeadingY = sin(MagHeading);
ErrorCourse = (DCM_M[0][0] * MagHeadingY) - (DCM_M[1][0] * MagHeadingX);
VScale(YawError,&DCM_M[2][0], ErrorCourse);
VScale(&Scaled_Omega_P[0], &YawError[0], Kp_Yaw);
VAdd(Omega_P, Omega_P, Scaled_Omega_P);
VScale(&Scaled_Omega_I[0], &YawError[0], Ki_Yaw);
VAdd(Omega_I,Omega_I, Scaled_Omega_I);
} // DriftCorrection
void Parametric_Normal(VECTOR Result, OBJECT* Object, INTERSECTION* Inter)
{
VECTOR RU, RV;
PARAMETRIC * Par = (PARAMETRIC *)Object;
VECTOR * IPoint = &(Inter->IPoint);
UV_VECT uv_vect;
uv_vect[U] = Par->last_u;
uv_vect[V] = Par->last_v;
RU[X] = RV[X] = -Evaluate_Function_UV(*(Par->Function[X]), uv_vect);
RU[Y] = RV[Y] = -Evaluate_Function_UV(*(Par->Function[Y]), uv_vect);
RU[Z] = RV[Z] = -Evaluate_Function_UV(*(Par->Function[Z]), uv_vect);
uv_vect[U] += Par->accuracy;
RU[X] += Evaluate_Function_UV(*(Par->Function[X]), uv_vect);
RU[Y] += Evaluate_Function_UV(*(Par->Function[Y]), uv_vect);
RU[Z] += Evaluate_Function_UV(*(Par->Function[Z]), uv_vect);
uv_vect[U] = Par->last_u;
uv_vect[V] += Par->accuracy;
RV[X] += Evaluate_Function_UV(*(Par->Function[X]), uv_vect);
RV[Y] += Evaluate_Function_UV(*(Par->Function[Y]), uv_vect);
RV[Z] += Evaluate_Function_UV(*(Par->Function[Z]), uv_vect);
VCross(Result, RU, RV);
if (Par->Trans != NULL)
MTransNormal(Result, Result, Par->Trans);
VNormalize(Result, Result);
}
void Parametric::Normal(VECTOR Result, Intersection *Inter, TraceThreadData *Thread) const
{
VECTOR RU, RV;
UV_VECT uv_vect;
uv_vect[U] = Inter->Iuv[U];
uv_vect[V] = Inter->Iuv[V];
RU[X] = RV[X] = -Evaluate_Function_UV(Thread->functionContext, *(Function[X]), uv_vect);
RU[Y] = RV[Y] = -Evaluate_Function_UV(Thread->functionContext, *(Function[Y]), uv_vect);
RU[Z] = RV[Z] = -Evaluate_Function_UV(Thread->functionContext, *(Function[Z]), uv_vect);
uv_vect[U] += accuracy;
RU[X] += Evaluate_Function_UV(Thread->functionContext, *(Function[X]), uv_vect);
RU[Y] += Evaluate_Function_UV(Thread->functionContext, *(Function[Y]), uv_vect);
RU[Z] += Evaluate_Function_UV(Thread->functionContext, *(Function[Z]), uv_vect);
uv_vect[U] = Inter->Iuv[U];
uv_vect[V] += accuracy;
RV[X] += Evaluate_Function_UV(Thread->functionContext, *(Function[X]), uv_vect);
RV[Y] += Evaluate_Function_UV(Thread->functionContext, *(Function[Y]), uv_vect);
RV[Z] += Evaluate_Function_UV(Thread->functionContext, *(Function[Z]), uv_vect);
VCross(Result, RU, RV);
if (Trans != NULL)
MTransNormal(Result, Result, Trans);
VNormalize(Result, Result);
}
void Normalize(void)
{
static float Error = 0.0;
static float Temp[3][3];
static float Renorm = 0.0;
static boolean Problem = FALSE;
static byte r;
Error= -VDot(&DCM_M[0][0], &DCM_M[1][0]) * 0.5; //eq.19
VScale(&Temp[0][0], &DCM_M[1][0], Error); //eq.19
VScale(&Temp[1][0], &DCM_M[0][0], Error); //eq.19
VAdd(&Temp[0][0], &Temp[0][0], &DCM_M[0][0]); //eq.19
VAdd(&Temp[1][0], &Temp[1][0], &DCM_M[1][0]); //eq.19
VCross(&Temp[2][0],&Temp[0][0], &Temp[1][0]); // c= a * b eq.20
#if EXTENDED == 1
for ( r= 0; r < 3; r++ )
{
Renorm= VDot(&Temp[r][0],&Temp[r][0]);
if ( Renorm < 1.5625 && Renorm > 0.64 )
Renorm = 0.5 * (3.0 - Renorm); //eq.21
else
if ( Renorm < 100.0 && Renorm > 0.01 )
Renorm = 1.0 / sqrt( Renorm );
else
Problem = TRUE;
VScale(&DCM_M[r][0], &Temp[r][0], Renorm);
}
if ( Problem ) // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
{
DCM_M[0][0] = 1.0;
DCM_M[0][1] = 0.0;
DCM_M[0][2] = 0.0;
DCM_M[1][0] = 0.0;
DCM_M[1][1] = 1.0;
DCM_M[1][2] = 0.0;
DCM_M[2][0] = 0.0;
DCM_M[2][1] = 0.0;
DCM_M[2][2] = 1.0;
Problem = TRUE;
}
#else
for ( r= 0; r < 3; r++ )
{
Renorm = 0.5 * (3.0 - VDot(&Temp[r][0], &Temp[r][0])); //eq.21
VScale(&DCM_M[r][0], &Temp[r][0], Renorm);
}
#endif // EXTENDED
} // Normalize
static void cylinder_intersect(cylinder * cyl, ray * ry) {
vector rc, n, D, O;
flt t, s, tin, tout, ln, d;
rc.x = ry->o.x - cyl->ctr.x;
rc.y = ry->o.y - cyl->ctr.y;
rc.z = ry->o.z - cyl->ctr.z;
VCross(&ry->d, &cyl->axis, &n);
VDOT(ln, n, n);
ln=sqrt(ln); /* finish length calculation */
if (ln == 0.0) { /* ray is parallel to the cylinder.. */
VDOT(d, rc, cyl->axis);
D.x = rc.x - d * cyl->axis.x;
D.y = rc.y - d * cyl->axis.y;
D.z = rc.z - d * cyl->axis.z;
VDOT(d, D, D);
d = sqrt(d);
tin = -FHUGE;
tout = FHUGE;
/* if (d <= cyl->rad) then ray is inside cylinder.. else outside */
}
VNorm(&n);
VDOT(d, rc, n);
d = fabs(d);
if (d <= cyl->rad) { /* ray intersects cylinder.. */
VCross(&rc, &cyl->axis, &O);
VDOT(t, O, n);
t = - t / ln;
VCross(&n, &cyl->axis, &O);
VNorm(&O);
VDOT(s, ry->d, O);
s = fabs(sqrt(cyl->rad*cyl->rad - d*d) / s);
tin = t - s;
add_intersection(tin, (object *) cyl, ry);
tout = t + s;
add_intersection(tout, (object *) cyl, ry);
}
}
void ComputeTorqs ()
{
VecR dr, t, torqS, waB;
int j, n;
DO_MOL {
VZero (mol[n].ra);
VZero (torqS);
for (j = 0; j < sitesMol; j ++) {
VVAdd (mol[n].ra, site[n * sitesMol + j].f);
VSub (dr, site[n * sitesMol + j].r, mol[n].r);
VCross (t, dr, site[n * sitesMol + j].f);
VVAdd (torqS, t);
}
MVMulT (waB, mol[n].rMatT.u, torqS);
VDiv (waB, waB, mInert);
MVMul (mol[n].wa, mol[n].rMatT.u, waB);
}
}
void ComputeTorqs ()
{
RMat rMat;
VecR dr, t, torqS;
int j, n;
DO_MOL {
VZero (mol[n].ra);
VZero (torqS);
for (j = 0; j < sitesMol; j ++) {
VVAdd (mol[n].ra, site[n * sitesMol + j].f);
VSub (dr, site[n * sitesMol + j].r, mol[n].r);
VCross (t, dr, site[n * sitesMol + j].f);
VVAdd (torqS, t);
}
BuildRotMatrix (&rMat, &mol[n].q, 0);
MVMul (mol[n].torq, rMat.u, torqS);
}
}
static int subpatch_normal(VECTOR v1, VECTOR v2, VECTOR v3, VECTOR Result, DBL *d)
{
VECTOR V1, V2;
DBL squared_v1, squared_v2;
DBL Length;
VSub(V1, v1, v2);
VSub(V2, v3, v2);
VCross(Result, V1, V2);
Length = VSumSqr(Result);
squared_v1 = VSumSqr(V1);
squared_v2 = VSumSqr(V2);
if (Length <= (BEZIER_EPSILON * squared_v1 * squared_v2))
{
Make_Vector(Result, 1.0, 0.0, 0.0);
*d = -1.0 * v1[X];
return (0);
}
else
{
Length = sqrt(Length);
VInverseScale(Result, Result, Length);
VDot(*d, Result, v1);
*d = 0.0 - *d;
return (1);
}
}
static int intersect_subpatch(BICUBIC_PATCH *Shape, RAY *ray, VECTOR V1[3], DBL uu[3], DBL vv[3], DBL *Depth, VECTOR P, VECTOR N, DBL *u, DBL *v)
{
DBL squared_b0, squared_b1;
DBL d, n, a, b, r;
VECTOR Q, T1;
VECTOR B[3], IB[3], NN[3];
VSub(B[0], V1[1], V1[0]);
VSub(B[1], V1[2], V1[0]);
VCross(B[2], B[0], B[1]);
VDot(d, B[2], B[2]);
squared_b0 = VSumSqr(B[0]);
squared_b1 = VSumSqr(B[1]);
if (d <= (BEZIER_EPSILON * squared_b1 * squared_b0))
{
return (0);
}
d = 1.0 / sqrt(d);
VScaleEq(B[2], d);
/* Degenerate triangle. */
if (!MInvers3(B, IB))
{
return (0);
}
VDot(d, ray->Direction, IB[2]);
if (fabs(d) < BEZIER_EPSILON)
{
return (0);
}
VSub(Q, V1[0], ray->Initial);
VDot(n, Q, IB[2]);
*Depth = n / d;
if (*Depth < BEZIER_TOLERANCE)
{
return (0);
}
VScale(T1, ray->Direction, *Depth);
VAdd(P, ray->Initial, T1);
VSub(Q, P, V1[0]);
VDot(a, Q, IB[0]);
VDot(b, Q, IB[1]);
if ((a < 0.0) || (b < 0.0) || (a + b > 1.0))
{
return (0);
}
r = 1.0 - a - b;
Make_Vector(N, 0.0, 0.0, 0.0);
bezier_value((VECTOR(*)[4][4])&Shape->Control_Points, uu[0], vv[0], T1, NN[0]);
bezier_value((VECTOR(*)[4][4])&Shape->Control_Points, uu[1], vv[1], T1, NN[1]);
bezier_value((VECTOR(*)[4][4])&Shape->Control_Points, uu[2], vv[2], T1, NN[2]);
VScale(T1, NN[0], r); VAddEq(N, T1);
VScale(T1, NN[1], a); VAddEq(N, T1);
VScale(T1, NN[2], b); VAddEq(N, T1);
*u = r * uu[0] + a * uu[1] + b * uu[2];
*v = r * vv[0] + a * vv[1] + b * vv[2];
VDot(d, N, N);
if (d > BEZIER_EPSILON)
{
d = 1.0 / sqrt(d);
VScaleEq(N, d);
}
else
{
Make_Vector(N, 1, 0, 0);
}
return (1);
}
void bump_map(VECTOR EPoint, TNORMAL *Tnormal, VECTOR normal)
{
DBL xcoor = 0.0, ycoor = 0.0;
int index, index2, index3;
COLOUR colour1, colour2, colour3;
VECTOR p1, p2, p3;
VECTOR bump_normal;
VECTOR xprime, yprime, zprime, Temp;
DBL Length;
DBL Amount = Tnormal->Amount;
IMAGE *Image = Tnormal->Vals.Image;
Make_ColourA(colour1, 0.0, 0.0, 0.0, 0.0, 0.0);
Make_ColourA(colour2, 0.0, 0.0, 0.0, 0.0, 0.0);
Make_ColourA(colour3, 0.0, 0.0, 0.0, 0.0, 0.0);
/* going to have to change this */
/* need to know if bump point is off of image for all 3 points */
if (map(EPoint, (TPATTERN *) Tnormal, &xcoor, &ycoor))
{
return;
}
else
{
image_colour_at(Image, xcoor, ycoor, colour1, &index);
}
xcoor--;
ycoor++;
if (xcoor < 0.0)
{
xcoor += (DBL)Image->iwidth;
}
else
{
if (xcoor >= Image->iwidth)
{
xcoor -= (DBL)Image->iwidth;
}
}
if (ycoor < 0.0)
{
ycoor += (DBL)Image->iheight;
}
else
{
if (ycoor >= (DBL)Image->iheight)
{
ycoor -= (DBL)Image->iheight;
}
}
image_colour_at(Image, xcoor, ycoor, colour2, &index2);
xcoor += 2.0;
if (xcoor < 0.0)
{
xcoor += (DBL)Image->iwidth;
}
else
{
if (xcoor >= Image->iwidth)
{
xcoor -= (DBL)Image->iwidth;
}
}
image_colour_at(Image, xcoor, ycoor, colour3, &index3);
if (Image->Colour_Map == NULL || Image->Use_Colour_Flag)
{
p1[X] = 0;
p1[Y] = Amount * (GREY_SCALE( colour1 ));
p1[Z] = 0;
p2[X] = -1;
p2[Y] = Amount * (GREY_SCALE( colour2 ));
p2[Z] = 1;
p3[X] = 1;
p3[Y] = Amount * (GREY_SCALE( colour3 ));
p3[Z] = 1;
}
else
{
p1[X] = 0;
p1[Y] = Amount * index;
p1[Z] = 0;
p2[X] = -1;
p2[Y] = Amount * index2;
p2[Z] = 1;
p3[X] = 1;
p3[Y] = Amount * index3;
p3[Z] = 1;
}
/* we have points 1,2,3 for a triangle now we need the surface normal for it */
VSub(xprime, p1, p2);
VSub(yprime, p3, p2);
VCross(bump_normal, yprime, xprime);
VNormalize(bump_normal, bump_normal);
Assign_Vector(yprime, normal);
Make_Vector(Temp, 0.0, 1.0, 0.0);
VCross(xprime, yprime, Temp);
VLength(Length, xprime);
if (Length < EPSILON)
{
if (fabs(normal[Y] - 1.0) < Small_Tolerance)
{
Make_Vector(yprime, 0.0, 1.0, 0.0);
Make_Vector(xprime, 1.0, 0.0, 0.0);
Length = 1.0;
}
else
{
Make_Vector(yprime, 0.0, -1.0, 0.0);
Make_Vector(xprime, 1.0, 0.0, 0.0);
Length = 1.0;
}
}
VScaleEq(xprime, 1.0 / Length);
VCross(zprime, xprime, yprime);
VNormalizeEq(zprime);
VScaleEq(xprime, bump_normal[X]);
VScaleEq(yprime, bump_normal[Y]);
VScaleEq(zprime, bump_normal[Z]);
VAdd(Temp, xprime, yprime);
VScaleEq(zprime, -1);
VAdd(normal, Temp, zprime);
}
void CameraWorkManager::Update_RotateCamOnGazePoint( double timeslice )
{
static const double DrugMoveScale=0.005; // ドラッグ量に対する視点の移動量のパラメータ
static Vector2D ScreenDrugStartPoint; // マウスでドラッグを開始したスクリーン上の座標を格納
static Vector2D SavedCamDist; // マウスでドラッグを開始したときの、m_dTilt、m_dHead を保持
static bool nowDruging=false; // 今ドラッグ中か？
// キー入力から、m_dTilt と m_dHead を更新
//getKeyInput();
// スクロール操作から、カメラと注視点の距離を更新
getMouseWheelInput(timeslice);
// マウスによるドラッグ操作により、m_dTilt と m_dHead を更新
if( nowDruging )
{ // 今ドラッグ中
// ドラッグ終了したかを判定
if( ( GetMouseInput() & MOUSE_INPUT_LEFT ) == 0 )
{ // ドラッグ終了
nowDruging = false;
}
else
{
// 今のマウス位置を取得
Point2D pos;
GetMousePoint( &(pos.x) , &(pos.y) );
Vector2D CurMousePos( (double)pos.x, (double)pos.y);
// m_dTilt と m_dHead を更新
Vector2D NewCamDist = -DrugMoveScale*(CurMousePos-ScreenDrugStartPoint)+SavedCamDist;
m_dTilt = NewCamDist.y;
m_dHead = NewCamDist.x;
}
}
else
{ // ドラッグ中でない
// ドラッグ開始したかを判定
int button;
Point2D pos;
if( GetMouseInputLog( &button, &pos.x, &pos.y, TRUE )==0
&& ( button & MOUSE_INPUT_LEFT ) != 0 )
{ // ドラッグが開始された
// ドラッグ開始時の マウス座標を ScreenDrugStartPoint に保持
ScreenDrugStartPoint = pos.toRealVector();
// ドラッグ開始時の m_dTilt、m_dHead の値を SavedCamDist に保持
SavedCamDist.y = m_dTilt;
SavedCamDist.x = m_dHead;
// ドラッグフラグを立てる
nowDruging = true;
}
}
// m_dHead と m_dTilt から、（GazingPointを原点とした）カメラの位置を計算する
Vector3D Hedding3D( 1.0, 0, 0 );
Hedding3D = VTransform( Hedding3D.toVECTOR(), MGetRotZ( (float)m_dTilt ) ); // もしかしたら、マイナスにしないと駄目かも
Hedding3D = VTransform( Hedding3D.toVECTOR(), MGetRotY( -1*(float)m_dHead ) );
Vector3D toCamPos3D = m_dCamDistFrmFP * Hedding3D ;
// ### ビュー行列を生成を確認
// カメラの座標変換行列のz軸向き（=カメラの向き）
Vector3D vViewBaseZ = -1 * Hedding3D;
// カメラの座標変換行列のy軸向きを仮置き
Vector3D vViewBaseY( 0.0, 1.0, 0.0 );
// カメラの座標変換行列のx軸向き（= y × z ）
Vector3D vViewBaseX = VCross( vViewBaseY.toVECTOR(), vViewBaseZ.toVECTOR() );
vViewBaseX = vViewBaseX.normalize();
// カメラの座標変換行列のy軸向きを再調整
vViewBaseY = VCross( vViewBaseZ.toVECTOR(), vViewBaseX.toVECTOR() );
// 基底を組み合わせてカメラ座標変換行列（Entityローカル座標）を生成
MATRIX MCamTransMatLocal =
MGetAxis1(
//MakeMatrixFromBaseVec(
vViewBaseX.toVECTOR(),
vViewBaseY.toVECTOR(),
vViewBaseZ.toVECTOR(),
toCamPos3D.toVECTOR() );
MCamTransMatLocal.m[3][1] += m_TrgtHight;
// Entity位置だけシフトしたものがワールド座標でのカメラ座標変換行列
MATRIX MCamTransMatWorld = MCamTransMatLocal;
MCamTransMatWorld.m[3][0] += m_TargetPos.x;
//MCamTransMatWorld.m[3][1] += m_TrgtHight; <- MCamTransMatLocal の段階でシフト
MCamTransMatWorld.m[3][2] += m_TargetPos.z;
// 逆行列がビュー行列
m_MViewLocal = MInverse( MCamTransMatLocal );
m_MViewWorld = MInverse( MCamTransMatWorld );
// 背景パノラマ球の描画に使用しているため m_vFinalCamPos を参照している
m_vFinalCamPos = VTransform( Vector3D( 0,0,0 ).toVECTOR(), MCamTransMatWorld );
};
static FeatureData
Combine_Line_Line_Constraints(FeaturePtr c1, FeaturePtr c2)
{
Vector cross;
double param1;
Vector temp_v1, temp_v2, temp_v3, temp_v4;
Vector point1;
FeatureData result;
/* The result is inconsistent if the lines don't intersect, a point
** otherwise (their point of intersection.
*/
VCross(c1->f_vector, c2->f_vector, cross);
if ( VZero(cross) )
{
/* The lines are parallel. */
if ( Point_On_Line( c1->f_vector, c1->f_point,
c2->f_point) )
return *c1;
else
{
result.f_type = inconsistent_feature;
return result;
}
}
/* They still may or may not intersect. */
VSub(c1->f_point, c2->f_point, temp_v1);
VCross(c2->f_vector, temp_v1, temp_v2);
VCross(c1->f_vector, temp_v1, temp_v3);
/* temp_v3 and temp_v2 are both perp to temp_v1. */
/* If temp_v2 is 0, then the lines intersect at c1->f_point1. */
/* If temp_v3 is 0, then the lines intersect at c1->f_point2. */
/* If temp_v2 and temp_v3 are parallel, the lines intersect somewhere. */
/* Otherwise they don't. */
if ( VZero(temp_v2) )
{
result.f_type = point_feature;
result.f_point = c1->f_point;
return result;
}
else if ( VZero(temp_v2) )
{
result.f_type = point_feature;
result.f_point = c2->f_point;
return result;
}
VCross(temp_v2, temp_v3, temp_v4);
if ( ! VZero(temp_v4) )
{
result.f_type = inconsistent_feature;
return result;
}
/* Get the parameter for the point of intersection. */
if ( ! IsZero(cross.z) )
param1 = temp_v2.z / cross.z;
else if ( ! IsZero(cross.y) )
param1 = temp_v2.y / cross.y;
else
param1 = temp_v2.x / cross.x;
VScalarMul(c1->f_vector, param1, temp_v1);
VAdd(temp_v1, c1->f_point, point1);
result.f_type = point_feature;
result.f_point = point1;
return result;
}
// プレイヤーの処理
void Player_Process( void )
{
VECTOR UpMoveVec ; // 方向ボタン「↑」を入力をしたときのプレイヤーの移動方向ベクトル
VECTOR LeftMoveVec ; // 方向ボタン「←」を入力をしたときのプレイヤーの移動方向ベクトル
VECTOR MoveVec ; // このフレームの移動ベクトル
int JumpFlag ; // ジャンプフラグ
int i ;
// プレイヤーの移動方向のベクトルを算出
{
// 方向ボタン「↑」を押したときのプレイヤーの移動ベクトルはカメラの視線方向からＹ成分を抜いたもの
UpMoveVec = VSub( cam.Target, cam.Eye ) ;
UpMoveVec.y = 0.0f ;
// 方向ボタン「←」を押したときのプレイヤーの移動ベクトルは上を押したときの方向ベクトルとＹ軸のプラス方向のベクトルに垂直な方向
LeftMoveVec = VCross( UpMoveVec, VGet( 0.0f, 1.0f, 0.0f ) ) ;
// 二つのベクトルを正規化( ベクトルの長さを１．０にすること )
UpMoveVec = VNorm( UpMoveVec ) ;
LeftMoveVec = VNorm( LeftMoveVec ) ;
}
// このフレームでの移動ベクトルを初期化
MoveVec = VGet( 0.0f, 0.0f, 0.0f ) ;
// ジャンプフラグを倒す
JumpFlag = 0 ;
// パッドの３ボタンと左シフトがどちらも押されていなかったらプレイヤーの移動処理
if( CheckHitKey( KEY_INPUT_LSHIFT ) == 0 && ( inp.NowInput & PAD_INPUT_C ) == 0 )
{
// 方向ボタン「←」が入力されたらカメラの見ている方向から見て左方向に移動する
if( inp.NowInput & PAD_INPUT_LEFT )
{
// 移動ベクトルに「←」が入力された時の移動ベクトルを加算する
MoveVec = VAdd( MoveVec, LeftMoveVec ) ;
}
else
// 方向ボタン「→」が入力されたらカメラの見ている方向から見て右方向に移動する
if( inp.NowInput & PAD_INPUT_RIGHT )
{
// 移動ベクトルに「←」が入力された時の移動ベクトルを反転したものを加算する
MoveVec = VAdd( MoveVec, VScale( LeftMoveVec, -1.0f ) ) ;
}
// 方向ボタン「↑」が入力されたらカメラの見ている方向に移動する
if( inp.NowInput & PAD_INPUT_UP )
{
// 移動ベクトルに「↑」が入力された時の移動ベクトルを加算する
MoveVec = VAdd( MoveVec, UpMoveVec ) ;
}
else
// 方向ボタン「↓」が入力されたらカメラの方向に移動する
if( inp.NowInput & PAD_INPUT_DOWN )
{
// 移動ベクトルに「↑」が入力された時の移動ベクトルを反転したものを加算する
MoveVec = VAdd( MoveVec, VScale( UpMoveVec, -1.0f ) ) ;
}
// ボタン１が押されていたらジャンプフラグを立てる
if( inp.EdgeInput & PAD_INPUT_A )
{
JumpFlag = 1 ;
}
}
// 移動方向を移動速度でスケーリングする
MoveVec = VScale( MoveVec, CHARA_MOVE_SPEED ) ;
// プレイヤーキャラ以外との当たり判定を行う
for( i = 0 ; i < NOTPLAYER_NUM ; i ++ )
{
Chara_Collision( &pl.CharaInfo, &MoveVec, &npl[ i ].CharaInfo ) ;
}
// キャラクターを動作させる処理を行う
Chara_Process( &pl.CharaInfo, MoveVec, JumpFlag ) ;
}
// キャラクターの移動処理
void Chara_Move( CHARA *ch, VECTOR MoveVector )
{
int i, j, k ; // 汎用カウンタ変数
int MoveFlag ; // 水平方向に移動したかどうかのフラグ( ０:移動していない １:移動した )
int HitFlag ; // ポリゴンに当たったかどうかを記憶しておくのに使う変数( ０:当たっていない １:当たった )
MV1_COLL_RESULT_POLY_DIM HitDim ; // キャラクターの周囲にあるポリゴンを検出した結果が代入される当たり判定結果構造体
int KabeNum ; // 壁ポリゴンと判断されたポリゴンの数
int YukaNum ; // 床ポリゴンと判断されたポリゴンの数
MV1_COLL_RESULT_POLY *Kabe[ CHARA_MAX_HITCOLL ] ; // 壁ポリゴンと判断されたポリゴンの構造体のアドレスを保存しておくためのポインタ配列
MV1_COLL_RESULT_POLY *Yuka[ CHARA_MAX_HITCOLL ] ; // 床ポリゴンと判断されたポリゴンの構造体のアドレスを保存しておくためのポインタ配列
MV1_COLL_RESULT_POLY *Poly ; // ポリゴンの構造体にアクセスするために使用するポインタ( 使わなくても済ませられますがプログラムが長くなるので・・・ )
HITRESULT_LINE LineRes ; // 線分とポリゴンとの当たり判定の結果を代入する構造体
VECTOR OldPos ; // 移動前の座標
VECTOR NowPos ; // 移動後の座標
// 移動前の座標を保存
OldPos = ch->Position ;
// 移動後の座標を算出
NowPos = VAdd( ch->Position, MoveVector ) ;
// キャラクターの周囲にあるステージポリゴンを取得する
// ( 検出する範囲は移動距離も考慮する )
HitDim = MV1CollCheck_Sphere( stg.ModelHandle, -1, ch->Position, CHARA_ENUM_DEFAULT_SIZE + VSize( MoveVector ) ) ;
// x軸かy軸方向に 0.01f 以上移動した場合は「移動した」フラグを１にする
if( fabs( MoveVector.x ) > 0.01f || fabs( MoveVector.z ) > 0.01f )
{
MoveFlag = 1 ;
}
else
{
MoveFlag = 0 ;
}
// 検出されたポリゴンが壁ポリゴン( ＸＺ平面に垂直なポリゴン )か床ポリゴン( ＸＺ平面に垂直ではないポリゴン )かを判断する
{
// 壁ポリゴンと床ポリゴンの数を初期化する
KabeNum = 0 ;
YukaNum = 0 ;
// 検出されたポリゴンの数だけ繰り返し
for( i = 0 ; i < HitDim.HitNum ; i ++ )
{
// ＸＺ平面に垂直かどうかはポリゴンの法線のＹ成分が０に限りなく近いかどうかで判断する
if( HitDim.Dim[ i ].Normal.y < 0.000001f && HitDim.Dim[ i ].Normal.y > -0.000001f )
{
// 壁ポリゴンと判断された場合でも、キャラクターのＹ座標＋１．０ｆより高いポリゴンのみ当たり判定を行う
if( HitDim.Dim[ i ].Position[ 0 ].y > ch->Position.y + 1.0f ||
HitDim.Dim[ i ].Position[ 1 ].y > ch->Position.y + 1.0f ||
HitDim.Dim[ i ].Position[ 2 ].y > ch->Position.y + 1.0f )
{
// ポリゴンの数が列挙できる限界数に達していなかったらポリゴンを配列に追加
if( KabeNum < CHARA_MAX_HITCOLL )
{
// ポリゴンの構造体のアドレスを壁ポリゴンポインタ配列に保存する
Kabe[ KabeNum ] = &HitDim.Dim[ i ] ;
// 壁ポリゴンの数を加算する
KabeNum ++ ;
}
}
}
else
{
// ポリゴンの数が列挙できる限界数に達していなかったらポリゴンを配列に追加
if( YukaNum < CHARA_MAX_HITCOLL )
{
// ポリゴンの構造体のアドレスを床ポリゴンポインタ配列に保存する
Yuka[ YukaNum ] = &HitDim.Dim[ i ] ;
// 床ポリゴンの数を加算する
YukaNum ++ ;
}
}
}
}
// 壁ポリゴンとの当たり判定処理
if( KabeNum != 0 )
{
// 壁に当たったかどうかのフラグは初期状態では「当たっていない」にしておく
HitFlag = 0 ;
// 移動したかどうかで処理を分岐
if( MoveFlag == 1 )
{
// 壁ポリゴンの数だけ繰り返し
for( i = 0 ; i < KabeNum ; i ++ )
{
// i番目の壁ポリゴンのアドレスを壁ポリゴンポインタ配列から取得
Poly = Kabe[ i ] ;
// ポリゴンとキャラクターが当たっていなかったら次のカウントへ
if( HitCheck_Capsule_Triangle( NowPos, VAdd( NowPos, VGet( 0.0f, CHARA_HIT_HEIGHT, 0.0f ) ), CHARA_HIT_WIDTH, Poly->Position[ 0 ], Poly->Position[ 1 ], Poly->Position[ 2 ] ) == FALSE ) continue ;
// ここにきたらポリゴンとキャラクターが当たっているということなので、ポリゴンに当たったフラグを立てる
HitFlag = 1 ;
// 壁に当たったら壁に遮られない移動成分分だけ移動する
{
VECTOR SlideVec ; // キャラクターをスライドさせるベクトル
// 進行方向ベクトルと壁ポリゴンの法線ベクトルに垂直なベクトルを算出
SlideVec = VCross( MoveVector, Poly->Normal ) ;
// 算出したベクトルと壁ポリゴンの法線ベクトルに垂直なベクトルを算出、これが
// 元の移動成分から壁方向の移動成分を抜いたベクトル
SlideVec = VCross( Poly->Normal, SlideVec ) ;
// それを移動前の座標に足したものを新たな座標とする
NowPos = VAdd( OldPos, SlideVec ) ;
}
// 新たな移動座標で壁ポリゴンと当たっていないかどうかを判定する
for( j = 0 ; j < KabeNum ; j ++ )
{
// j番目の壁ポリゴンのアドレスを壁ポリゴンポインタ配列から取得
Poly = Kabe[ j ] ;
// 当たっていたらループから抜ける
if( HitCheck_Capsule_Triangle( NowPos, VAdd( NowPos, VGet( 0.0f, CHARA_HIT_HEIGHT, 0.0f ) ), CHARA_HIT_WIDTH, Poly->Position[ 0 ], Poly->Position[ 1 ], Poly->Position[ 2 ] ) == TRUE ) break ;
}
// j が KabeNum だった場合はどのポリゴンとも当たらなかったということなので
// 壁に当たったフラグを倒した上でループから抜ける
if( j == KabeNum )
{
HitFlag = 0 ;
break ;
}
}
}
else
{
// 移動していない場合の処理
// 壁ポリゴンの数だけ繰り返し
for( i = 0 ; i < KabeNum ; i ++ )
{
// i番目の壁ポリゴンのアドレスを壁ポリゴンポインタ配列から取得
Poly = Kabe[ i ] ;
// ポリゴンに当たっていたら当たったフラグを立てた上でループから抜ける
if( HitCheck_Capsule_Triangle( NowPos, VAdd( NowPos, VGet( 0.0f, CHARA_HIT_HEIGHT, 0.0f ) ), CHARA_HIT_WIDTH, Poly->Position[ 0 ], Poly->Position[ 1 ], Poly->Position[ 2 ] ) == TRUE )
{
HitFlag = 1 ;
break ;
}
}
}
// 壁に当たっていたら壁から押し出す処理を行う
if( HitFlag == 1 )
{
// 壁からの押し出し処理を試みる最大数だけ繰り返し
for( k = 0 ; k < CHARA_HIT_TRYNUM ; k ++ )
{
// 壁ポリゴンの数だけ繰り返し
for( i = 0 ; i < KabeNum ; i ++ )
{
// i番目の壁ポリゴンのアドレスを壁ポリゴンポインタ配列から取得
Poly = Kabe[ i ] ;
// キャラクターと当たっているかを判定
if( HitCheck_Capsule_Triangle( NowPos, VAdd( NowPos, VGet( 0.0f, CHARA_HIT_HEIGHT, 0.0f ) ), CHARA_HIT_WIDTH, Poly->Position[ 0 ], Poly->Position[ 1 ], Poly->Position[ 2 ] ) == FALSE ) continue ;
// 当たっていたら規定距離分キャラクターを壁の法線方向に移動させる
NowPos = VAdd( NowPos, VScale( Poly->Normal, CHARA_HIT_SLIDE_LENGTH ) ) ;
// 移動した上で壁ポリゴンと接触しているかどうかを判定
for( j = 0 ; j < KabeNum ; j ++ )
{
// 当たっていたらループを抜ける
Poly = Kabe[ j ] ;
if( HitCheck_Capsule_Triangle( NowPos, VAdd( NowPos, VGet( 0.0f, CHARA_HIT_HEIGHT, 0.0f ) ), CHARA_HIT_WIDTH, Poly->Position[ 0 ], Poly->Position[ 1 ], Poly->Position[ 2 ] ) == TRUE ) break ;
}
// 全てのポリゴンと当たっていなかったらここでループ終了
if( j == KabeNum ) break ;
}
// i が KabeNum ではない場合は全部のポリゴンで押し出しを試みる前に全ての壁ポリゴンと接触しなくなったということなのでループから抜ける
if( i != KabeNum ) break ;
}
}
}
// 床ポリゴンとの当たり判定
if( YukaNum != 0 )
{
// ジャンプ中且つ上昇中の場合は処理を分岐
if( ch->State == 2 && ch->JumpPower > 0.0f )
{
float MinY ;
// 天井に頭をぶつける処理を行う
// 一番低い天井にぶつける為の判定用変数を初期化
MinY = 0.0f ;
// 当たったかどうかのフラグを当たっていないを意味する０にしておく
HitFlag = 0 ;
// 床ポリゴンの数だけ繰り返し
for( i = 0 ; i < YukaNum ; i ++ )
{
// i番目の床ポリゴンのアドレスを床ポリゴンポインタ配列から取得
Poly = Yuka[ i ] ;
// 足先から頭の高さまでの間でポリゴンと接触しているかどうかを判定
LineRes = HitCheck_Line_Triangle( NowPos, VAdd( NowPos, VGet( 0.0f, CHARA_HIT_HEIGHT, 0.0f ) ), Poly->Position[ 0 ], Poly->Position[ 1 ], Poly->Position[ 2 ] ) ;
// 接触していなかったら何もしない
if( LineRes.HitFlag == FALSE ) continue ;
// 既にポリゴンに当たっていて、且つ今まで検出した天井ポリゴンより高い場合は何もしない
if( HitFlag == 1 && MinY < LineRes.Position.y ) continue ;
// ポリゴンに当たったフラグを立てる
HitFlag = 1 ;
// 接触したＹ座標を保存する
MinY = LineRes.Position.y ;
}
// 接触したポリゴンがあったかどうかで処理を分岐
if( HitFlag == 1 )
{
// 接触した場合はキャラクターのＹ座標を接触座標を元に更新
NowPos.y = MinY - CHARA_HIT_HEIGHT ;
// Ｙ軸方向の速度は反転
ch->JumpPower = -ch->JumpPower ;
}
}
else
{
float MaxY ;
// 下降中かジャンプ中ではない場合の処理
// 床ポリゴンに当たったかどうかのフラグを倒しておく
HitFlag = 0 ;
// 一番高い床ポリゴンにぶつける為の判定用変数を初期化
MaxY = 0.0f ;
// 床ポリゴンの数だけ繰り返し
for( i = 0 ; i < YukaNum ; i ++ )
{
// i番目の床ポリゴンのアドレスを床ポリゴンポインタ配列から取得
Poly = Yuka[ i ] ;
// ジャンプ中かどうかで処理を分岐
if( ch->State == 2 )
{
// ジャンプ中の場合は頭の先から足先より少し低い位置の間で当たっているかを判定
LineRes = HitCheck_Line_Triangle( VAdd( NowPos, VGet( 0.0f, CHARA_HIT_HEIGHT, 0.0f ) ), VAdd( NowPos, VGet( 0.0f, -1.0f, 0.0f ) ), Poly->Position[ 0 ], Poly->Position[ 1 ], Poly->Position[ 2 ] ) ;
}
else
{
// 走っている場合は頭の先からそこそこ低い位置の間で当たっているかを判定( 傾斜で落下状態に移行してしまわない為 )
LineRes = HitCheck_Line_Triangle( VAdd( NowPos, VGet( 0.0f, CHARA_HIT_HEIGHT, 0.0f ) ), VAdd( NowPos, VGet( 0.0f, -40.0f, 0.0f ) ), Poly->Position[ 0 ], Poly->Position[ 1 ], Poly->Position[ 2 ] ) ;
}
// 当たっていなかったら何もしない
if( LineRes.HitFlag == FALSE ) continue ;
// 既に当たったポリゴンがあり、且つ今まで検出した床ポリゴンより低い場合は何もしない
if( HitFlag == 1 && MaxY > LineRes.Position.y ) continue ;
// ポリゴンに当たったフラグを立てる
HitFlag = 1 ;
// 接触したＹ座標を保存する
MaxY = LineRes.Position.y ;
}
// 床ポリゴンに当たったかどうかで処理を分岐
if( HitFlag == 1 )
{
// 当たった場合
// 接触したポリゴンで一番高いＹ座標をキャラクターのＹ座標にする
NowPos.y = MaxY ;
// Ｙ軸方向の移動速度は０に
ch->JumpPower = 0.0f ;
// もしジャンプ中だった場合は着地状態にする
if( ch->State == 2 )
{
// 移動していたかどうかで着地後の状態と再生するアニメーションを分岐する
if( MoveFlag )
{
// 移動している場合は走り状態に
Chara_PlayAnim( ch, 1 ) ;
ch->State = 1 ;
}
else
{
// 移動していない場合は立ち止り状態に
Chara_PlayAnim( ch, 4 ) ;
ch->State = 0 ;
}
// 着地時はアニメーションのブレンドは行わない
ch->AnimBlendRate = 1.0f ;
}
}
else
{
// 床コリジョンに当たっていなくて且つジャンプ状態ではなかった場合は
if( ch->State != 2 )
{
// ジャンプ中にする
ch->State = 2 ;
// ちょっとだけジャンプする
ch->JumpPower = CHARA_FALL_UP_POWER ;
// アニメーションは落下中のものにする
Chara_PlayAnim( ch, 3 ) ;
}
}
}
}
// 新しい座標を保存する
ch->Position = NowPos ;
// キャラクターのモデルの座標を更新する
MV1SetPosition( ch->ModelHandle, ch->Position ) ;
// 検出したキャラクターの周囲のポリゴン情報を開放する
MV1CollResultPolyDimTerminate( HitDim ) ;
}
int Compute_Triangle(TRIANGLE *Triangle,int Smooth)
{
int swap,degn;
VECTOR V1, V2, Temp;
DBL Length;
VSub(V1, Triangle->P1, Triangle->P2);
VSub(V2, Triangle->P3, Triangle->P2);
VCross(Triangle->Normal_Vector, V1, V2);
VLength(Length, Triangle->Normal_Vector);
/* Set up a flag so we can ignore degenerate triangles */
if (Length == 0.0)
{
Set_Flag(Triangle, DEGENERATE_FLAG);
return(false);
}
/* Normalize the normal vector. */
VInverseScaleEq(Triangle->Normal_Vector, Length);
VDot(Triangle->Distance, Triangle->Normal_Vector, Triangle->P1);
Triangle->Distance *= -1.0;
find_triangle_dominant_axis(Triangle);
swap = false;
switch (Triangle->Dominant_Axis)
{
case X:
if ((Triangle->P2[Y] - Triangle->P3[Y])*(Triangle->P2[Z] - Triangle->P1[Z]) <
(Triangle->P2[Z] - Triangle->P3[Z])*(Triangle->P2[Y] - Triangle->P1[Y]))
{
swap = true;
}
break;
case Y:
if ((Triangle->P2[X] - Triangle->P3[X])*(Triangle->P2[Z] - Triangle->P1[Z]) <
(Triangle->P2[Z] - Triangle->P3[Z])*(Triangle->P2[X] - Triangle->P1[X]))
{
swap = true;
}
break;
case Z:
if ((Triangle->P2[X] - Triangle->P3[X])*(Triangle->P2[Y] - Triangle->P1[Y]) <
(Triangle->P2[Y] - Triangle->P3[Y])*(Triangle->P2[X] - Triangle->P1[X]))
{
swap = true;
}
break;
}
if (swap)
{
Assign_Vector(Temp, Triangle->P2);
Assign_Vector(Triangle->P2, Triangle->P1);
Assign_Vector(Triangle->P1, Temp);
if (Smooth)
{
Assign_Vector(Temp, ((SMOOTH_TRIANGLE *)Triangle)->N2);
Assign_Vector(((SMOOTH_TRIANGLE *)Triangle)->N2, ((SMOOTH_TRIANGLE *)Triangle)->N1);
Assign_Vector(((SMOOTH_TRIANGLE *)Triangle)->N1, Temp);
}
}
degn=true;
if (Smooth)
{
degn=compute_smooth_triangle((SMOOTH_TRIANGLE *)Triangle);
}
/* Build the bounding information from the vertices. */
Compute_Triangle_BBox(Triangle);
return(degn);
}
static void bezier_value(VECTOR (*Control_Points)[4][4], DBL u0, DBL v0, VECTOR P, VECTOR N)
{
const DBL C[] = { 1.0, 3.0, 3.0, 1.0 };
int i, j;
DBL c, t, ut, vt;
DBL u[4], uu[4], v[4], vv[4];
DBL du[4], duu[4], dv[4], dvv[4];
DBL squared_u1, squared_v1;
VECTOR U1, V1;
/* Calculate binomial coefficients times coordinate positions. */
u[0] = 1.0; uu[0] = 1.0; du[0] = 0.0; duu[0] = 0.0;
v[0] = 1.0; vv[0] = 1.0; dv[0] = 0.0; dvv[0] = 0.0;
for (i = 1; i < 4; i++)
{
u[i] = u[i - 1] * u0; uu[i] = uu[i - 1] * (1.0 - u0);
v[i] = v[i - 1] * v0; vv[i] = vv[i - 1] * (1.0 - v0);
du[i] = i * u[i - 1]; duu[i] = -i * uu[i - 1];
dv[i] = i * v[i - 1]; dvv[i] = -i * vv[i - 1];
}
/* Now evaluate position and tangents based on control points. */
Make_Vector(P, 0, 0, 0);
Make_Vector(U1, 0, 0, 0);
Make_Vector(V1, 0, 0, 0);
for (i = 0; i < 4; i++)
{
for (j = 0; j < 4; j++)
{
c = C[i] * C[j];
ut = u[i] * uu[3 - i];
vt = v[j] * vv[3 - j];
t = c * ut * vt;
VAddScaledEq(P, t, (*Control_Points)[i][j]);
t = c * vt * (du[i] * uu[3 - i] + u[i] * duu[3 - i]);
VAddScaledEq(U1, t, (*Control_Points)[i][j]);
t = c * ut * (dv[j] * vv[3 - j] + v[j] * dvv[3 - j]);
VAddScaledEq(V1, t, (*Control_Points)[i][j]);
}
}
/* Make the normal from the cross product of the tangents. */
VCross(N, U1, V1);
VDot(t, N, N);
squared_u1 = VSumSqr(U1);
squared_v1 = VSumSqr(V1);
if (t > (BEZIER_EPSILON * squared_u1 * squared_v1))
{
t = 1.0 / sqrt(t);
VScaleEq(N, t);
}
else
{
Make_Vector(N, 1, 0, 0);
}
}
void Compute_Polygon(POLYGON *Polyg, int Number, VECTOR *Points)
{
int i;
DBL x, y, z, d;
VECTOR o, u, v, w, N;
MATRIX a, b;
/* Create polygon data. */
if (Polyg->Data == NULL)
{
Polyg->Data = (POLYGON_DATA *)POV_MALLOC(sizeof(POLYGON_DATA), "polygon points");
Polyg->Data->References = 1;
Polyg->Data->Number = Number;
Polyg->Data->Points = (UV_VECT *)POV_MALLOC(Number*sizeof(UV_VECT), "polygon points");
}
else
{
Error("Polygon data already computed.");
}
/* Get polygon's coordinate system (one of the many possible) */
Assign_Vector(o, Points[0]);
/* Find valid, i.e. non-zero u vector. */
for (i = 1; i < Number; i++)
{
VSub(u, Points[i], o);
if (VSumSqr(u) > EPSILON)
{
break;
}
}
if (i == Number)
{
Set_Flag(Polyg, DEGENERATE_FLAG);
Warning(0, "Points in polygon are co-linear. Ignoring polygon.");
}
/* Find valid, i.e. non-zero v and w vectors. */
for (i++; i < Number; i++)
{
VSub(v, Points[i], o);
VCross(w, u, v);
if ((VSumSqr(v) > EPSILON) && (VSumSqr(w) > EPSILON))
{
break;
}
}
if (i == Number)
{
Set_Flag(Polyg, DEGENERATE_FLAG);
Warning(0, "Points in polygon are co-linear. Ignoring polygon.");
}
VCross(u, v, w);
VCross(v, w, u);
VNormalize(u, u);
VNormalize(v, v);
VNormalize(w, w);
MIdentity(a);
MIdentity(b);
a[3][0] = -o[X];
a[3][1] = -o[Y];
a[3][2] = -o[Z];
b[0][0] = u[X];
b[1][0] = u[Y];
b[2][0] = u[Z];
b[0][1] = v[X];
b[1][1] = v[Y];
b[2][1] = v[Z];
b[0][2] = w[X];
b[1][2] = w[Y];
b[2][2] = w[Z];
MTimesC(Polyg->Trans->inverse, a, b);
MInvers(Polyg->Trans->matrix, Polyg->Trans->inverse);
/* Project points onto the u,v-plane (3D --> 2D) */
for (i = 0; i < Number; i++)
{
x = Points[i][X] - o[X];
y = Points[i][Y] - o[Y];
z = Points[i][Z] - o[Z];
d = x * w[X] + y * w[Y] + z * w[Z];
if (fabs(d) > ZERO_TOLERANCE)
{
Set_Flag(Polyg, DEGENERATE_FLAG);
Warning(0, "Points in polygon are not co-planar. Ignoring polygons.");
}
Polyg->Data->Points[i][X] = x * u[X] + y * u[Y] + z * u[Z];
Polyg->Data->Points[i][Y] = x * v[X] + y * v[Y] + z * v[Z];
}
Make_Vector(N, 0.0, 0.0, 1.0);
MTransNormal(Polyg->S_Normal, N, Polyg->Trans);
VNormalizeEq(Polyg->S_Normal);
Compute_Polygon_BBox(Polyg);
}
