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; }