void PointSet::GetCenteredPoint(int index, float* p)
{
Point4D relative = points[index] - center;
p[0] = (float)(relative.GetPoint(0)/SCALE);
p[1] = (float)(relative.GetPoint(1)/SCALE);
p[2] = (float)(relative.GetPoint(2)/SCALE);
return;
}
void PointSet::AddPoint(Point4D& p)
{
if ( count==capacity ) MemAlloc();
points[count] = p;
center = Point4D(
(center.GetPoint(0)*count+p.GetPoint(0))/(count+1),
(center.GetPoint(1)*count+p.GetPoint(1))/(count+1),
(center.GetPoint(2)*count+p.GetPoint(2))/(count+1) );
count++;
}
bool VART::Human::ReachedDestiantion() const
{
static const float delta = 1.4;
Point4D location = adjustment * position * orientation * Point4D::ORIGIN();
Point4D distanceVector = destination - location;
//cout << "From " << location << " to " << destination << "\n";
distanceVector.SetY(0);
float distance = abs(distanceVector.Length());
//~ cout << "distance: " << distance << "\n";
return (distance < delta);
}
void Layer::drawOrbit(double orbitAngle) {
GLfloat WHITE [] = {1.0, 1.0, 1.0};
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, WHITE);
for(int i = 0; i < 360; i+= 5) {
glBegin(GL_POINTS);
Point4D positioninOrbit = getPositionInOrbit(i, radius, orbitAngle);
glVertex3d(positioninOrbit.GetX(), positioninOrbit.GetY(), positioninOrbit.GetZ());
glEnd();
}
}
void VART::Human::StickLeftFoot()
{
// Right foot is backwards. Adjustment brings it forward, so...
position.CopyMatrix(adjustment * position); // ...save current adjustment to human position
pathToStickPosPtr = &pathToLFoot;
stickPositionPtr = &lfFront; // now the left foot sticks to ground, but it is forward ...
// ... adjustment will bring it backwards, so we compute how ...
Point4D leftFootOffset; // ... much forward it is
ComputeLocalStickPosition(&leftFootOffset); // ... and add to position as well ...
Transform trans; // ... as vector, under influence of the human's orientation.
Point4D forward = position * orientation * (leftFootOffset - lfFront);
forward.SetY(-adjustment.GetData()[13]);
trans.MakeTranslation(forward);
position.CopyMatrix(trans * position);
}
Rect Matrix4x4::ProjectRectBounds(const Rect& aRect) const
{
Point4D points[4];
points[0] = ProjectPoint(aRect.TopLeft());
points[1] = ProjectPoint(aRect.TopRight());
points[2] = ProjectPoint(aRect.BottomRight());
points[3] = ProjectPoint(aRect.BottomLeft());
Float min_x = std::numeric_limits<Float>::max();
Float min_y = std::numeric_limits<Float>::max();
Float max_x = -std::numeric_limits<Float>::max();
Float max_y = -std::numeric_limits<Float>::max();
bool foundPoint = false;
for (int i=0; i<4; i++) {
// Only use points that exist above the w=0 plane
if (points[i].HasPositiveWCoord()) {
foundPoint = true;
Point point2d = points[i].As2DPoint();
min_x = min<Float>(point2d.x, min_x);
max_x = max<Float>(point2d.x, max_x);
min_y = min<Float>(point2d.y, min_y);
max_y = max<Float>(point2d.y, max_y);
}
int next = (i == 3) ? 0 : i + 1;
if (points[i].HasPositiveWCoord() != points[next].HasPositiveWCoord()) {
// If the line between two points crosses the w=0 plane, then interpolate a point
// as close to the w=0 plane as possible and use that instead.
Point4D intercept = ComputePerspectivePlaneIntercept(points[i], points[next]);
Point point2d = intercept.As2DPoint();
min_x = min<Float>(point2d.x, min_x);
max_x = max<Float>(point2d.x, max_x);
min_y = min<Float>(point2d.y, min_y);
max_y = max<Float>(point2d.y, max_y);
}
}
if (!foundPoint) {
return Rect(0, 0, 0, 0);
}
return Rect(min_x, min_y, max_x - min_x, max_y - min_y);
}
float VART::Human::AngleToPosition(const Point4D& pos, float* totalAngle) const
// Compute Angle to a position (pos) in World coordinates
{
static const float delta = 3.5; // distances smaller than delta are to small to make sharp turns
Point4D direction = pos - Position();
direction.SetY(0);
float stepAngle = maxStepRotation;
if (direction.Length() < delta) // if too close to destination, make only a small adjustment
stepAngle *= 0.01;
float angle = direction.GenericAngleTo(forward);
// Compute the Y coordinate of the cross product (we are working on world coordinates)
// between forward and direction. If it is negative, the angle is also negative.
if ((forward.GetZ()*direction.GetX() - forward.GetX()*direction.GetZ()) < 0)
{ // Negative angle
if (totalAngle)
*totalAngle = -angle;
if (angle > stepAngle)
return -stepAngle;
else
return -angle;
}
else
{ // Positive angle
if (totalAngle)
*totalAngle = angle;
if (angle > stepAngle)
return stepAngle;
else
return angle;
}
}
size_t
Matrix4x4::TransformAndClipRect(const Rect& aRect, const Rect& aClip,
Point* aVerts) const
{
// Initialize a double-buffered array of points in homogenous space with
// the input rectangle, aRect.
Point4D points[2][kTransformAndClipRectMaxVerts];
Point4D* dstPoint = points[0];
*dstPoint++ = *this * Point4D(aRect.x, aRect.y, 0, 1);
*dstPoint++ = *this * Point4D(aRect.XMost(), aRect.y, 0, 1);
*dstPoint++ = *this * Point4D(aRect.XMost(), aRect.YMost(), 0, 1);
*dstPoint++ = *this * Point4D(aRect.x, aRect.YMost(), 0, 1);
// View frustum clipping planes are described as normals originating from
// the 0,0,0,0 origin.
Point4D planeNormals[4];
planeNormals[0] = Point4D(1.0, 0.0, 0.0, -aClip.x);
planeNormals[1] = Point4D(-1.0, 0.0, 0.0, aClip.XMost());
planeNormals[2] = Point4D(0.0, 1.0, 0.0, -aClip.y);
planeNormals[3] = Point4D(0.0, -1.0, 0.0, aClip.YMost());
// Iterate through each clipping plane and clip the polygon.
// In each pass, we double buffer, alternating between points[0] and
// points[1].
for (int plane=0; plane < 4; plane++) {
planeNormals[plane].Normalize();
Point4D* srcPoint = points[plane & 1];
Point4D* srcPointEnd = dstPoint;
dstPoint = points[~plane & 1];
Point4D* prevPoint = srcPointEnd - 1;
float prevDot = planeNormals[plane].DotProduct(*prevPoint);
while (srcPoint < srcPointEnd) {
float nextDot = planeNormals[plane].DotProduct(*srcPoint);
if ((nextDot >= 0.0) != (prevDot >= 0.0)) {
// An intersection with the clipping plane has been detected.
// Interpolate to find the intersecting point and emit it.
float t = -prevDot / (nextDot - prevDot);
*dstPoint++ = *srcPoint * t + *prevPoint * (1.0 - t);
}
if (nextDot >= 0.0) {
// Emit any source points that are on the positive side of the
// clipping plane.
*dstPoint++ = *srcPoint;
}
prevPoint = srcPoint++;
prevDot = nextDot;
}
}
size_t dstPointCount = 0;
size_t srcPointCount = dstPoint - points[0];
for (Point4D* srcPoint = points[0]; srcPoint < points[0] + srcPointCount; srcPoint++) {
Point p;
if (srcPoint->w == 0.0) {
// If a point lies on the intersection of the clipping planes at
// (0,0,0,0), we must avoid a division by zero w component.
p = Point(0.0, 0.0);
} else {
p = srcPoint->As2DPoint();
}
// Emit only unique points
if (dstPointCount == 0 || p != aVerts[dstPointCount - 1]) {
aVerts[dstPointCount++] = p;
}
}
return dstPointCount;
}
void PointSet::CreateDelaunayOnSphere(PointSet* ps, FaceSet* fs, double* progress, std::stringstream* myMsg)
{
double dummy = 0;
if ( progress==NULL ) progress = &dummy;
int p_num = ps->GetCount();
PointSet tmpPS;
tmpPS.count = p_num;
tmpPS.capacity = p_num*2+4;
tmpPS.points = new Point4D [tmpPS.capacity];
const double OVERWRAP = 0.1;
int* OverwrapID = new int [p_num*2];
memset( OverwrapID, -1, sizeof(int)*2*p_num );
double center[3];
// Estimate Center
{
double pntRange[] = {
DBL_MAX, DBL_MAX, DBL_MAX,
-DBL_MAX, -DBL_MAX, -DBL_MAX,
};
for ( int i=0 ; i < p_num; i++ ) {
Point4D pnt = ps->GetPoint4DAt(i);
for ( int j=0; j < 3; j++ ) {
pntRange[j+0] = ( pntRange[j+0]>pnt.GetPoint(j) ? pnt.GetPoint(j) : pntRange[j+0]);
pntRange[j+3] = ( pntRange[j+3]<pnt.GetPoint(j) ? pnt.GetPoint(j) : pntRange[j+3]);
}
}
for ( int j=0; j < 3; j++ ) {
center[j] = ( pntRange[j+3]+pntRange[j+0] )/2;
}
}
// Project onto Sphere
{
Point4D Center( center[0], center[1], center[2] );
for ( int i=0; i < p_num; i++ ) {
double CntPnt[3];
(ps->GetPoint4DAt(i)-Center).GetPoint(CntPnt);
double lng = atan(CntPnt[1]/CntPnt[2])+( CntPnt[2]>0 ? 0 : M_PI ) +M_PI/2;
double lat = atan( CntPnt[0]/sqrt(CntPnt[1]*CntPnt[1]+CntPnt[2]*CntPnt[2]) ) +M_PI/2;
OverwrapID[i] = i;
tmpPS.points[i].SetPoint( lng, lat, lng*lng+lat*lat);
if ( lng < OVERWRAP ) {
OverwrapID[tmpPS.count] = i;
tmpPS.points[tmpPS.count++].SetPoint( lng+2*M_PI, lat, (lng+2*M_PI)*(lng+2*M_PI)+lat*lat );
}
}
}
// 外接4角形の作成
tmpPS.count += 4;
tmpPS.points[tmpPS.count-4].SetPoint( 6*M_PI, 2*M_PI, M_PI*M_PI*40 );
tmpPS.points[tmpPS.count-3].SetPoint( 6*M_PI, -1*M_PI, M_PI*M_PI*37 );
tmpPS.points[tmpPS.count-2].SetPoint( -2*M_PI, -1*M_PI, M_PI*M_PI*5 );
tmpPS.points[tmpPS.count-1].SetPoint( -2*M_PI, 2*M_PI, M_PI*M_PI*8 );
FaceSet myFS;
myFS.AddNewFace( tmpPS.count-2, tmpPS.count-3, tmpPS.count-4 );
myFS.AddNewFace( tmpPS.count-4, tmpPS.count-1, tmpPS.count-2 );
if ( !myFS.CheckDelaunay( &tmpPS ) ) {
// AfxMessageBox("Invalid initial Delaunay.", MB_ICONERROR);
// return;
throw "Invalid initial Delaunay.";
}
myFS.MakeDelaunay( &tmpPS, progress, myMsg );
for ( int i=1; i <= 4; i++ ) {
myFS.RemoveFaceV(tmpPS.count-i);
}
// IDの整理
{
int f_num = myFS.GetFaceCount(), *f_list=NULL ;
myFS.GetFaceIDList( &f_list, true );
for ( int i=0; i < f_num; i++ ) {
myFS.RemoveFace(i);
ASSERT(OverwrapID[f_list[i*3+0]]!=-1);
ASSERT(OverwrapID[f_list[i*3+1]]!=-1);
ASSERT(OverwrapID[f_list[i*3+2]]!=-1);
myFS.AddNewFace( OverwrapID[f_list[i*3+0]], OverwrapID[f_list[i*3+1]], OverwrapID[f_list[i*3+2]] );
}
delete [] f_list;
}
myFS.Unduplication();
delete [] OverwrapID;
*fs = myFS;
}
