/// <summary>
/// For now this is the editing mode for the currently selected support
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void glControl1_Click(object sender, EventArgs e)
{
// single click on GL Control
Object3d obj = UVDLPApp.Instance().SelectedObject;
if (obj == null)
{
return;
}
if (ctrldown == false)
{
return; // ctrl need to be held down
}
// this object is a support
if (obj.tag == Object3d.OBJ_SUPPORT)
{
Support sup = (Support)obj;// we can cast safely
// now we have to see if we clicked on an object
MouseEventArgs me = e as MouseEventArgs;
MouseButtons buttonPushed = me.Button;
int xPos = me.X;
int yPos = me.Y;
List <ISectData> isects = TestHitTest(xPos, yPos);
if (isects.Count == 0)
{
return; // no intersections
}
ISectData isd1 = null;
foreach (ISectData isd in isects)
{
// find the closest object we clicked
if (isd.obj.tag == Object3d.OBJ_NORMAL)
{
isd1 = isd; // save it
break;
}
}
if (isd1 == null)
{
return; // no object intersection
}
isd1.poly.CalcNormal();
m_isectnormal.x = isd1.poly.m_normal.x;
m_isectnormal.y = isd1.poly.m_normal.y;
m_isectnormal.z = isd1.poly.m_normal.z;
// ok, we've got the normal, we know where we've intersected
// my best guess is that we should move the support 5mm in the direction of the camera
// the tip of the support should touch the intersection point
// let's start with scaling the height...
//sup.ScaleToHeight(isd1.intersect.z);
//m_camera.m_eye
Engine3D.Vector3d towardseye = new Engine3D.Vector3d();
towardseye = m_isectnormal; // -m_camera.m_eye;
towardseye.Normalize(); // make the unit length of 1
towardseye.Scale(4.0f); // scale to 5 mm
sup.MoveFromTip(isd1.intersect, towardseye);
UpdateView();
//sup.
}
}
public void RotateRight(float deg)
{
Rotate(m_up, deg);
// update target and up
m_target = m_lookat - m_eye; // The "look-at" unit vector.
m_target.Normalize();
m_right = Vector3d.cross(m_target, m_up);
UpdateView();
}
public static List<ISectData> IntersectObjects(Vector3d direction, Point3d origin, List<Object3d> objects, bool supports)
{
//List<ISectData> m_isectlst = new List<ISectData>();
try
{
if (!vecinit)
{
Initvecs();
}
m_isectlst.Clear();
direction.Normalize();
direction.Scale(10000.0f);
IOendp.Set(origin);
IOendp.x += direction.x;
IOendp.y += direction.y;
IOendp.z += direction.z;
lock (lck)
{
foreach (Object3d obj in objects)
{
if (obj.tag == Object3d.OBJ_SUPPORT && !supports)
continue;
// try a less- costly sphere intersect here
if (IntersectSphere(origin, IOendp, ref IOintersect, obj.m_center, obj.m_radius))
{
foreach (Polygon p in obj.m_lstpolys)
{
//IOintersect = new Point3d();
// try a less- costly sphere intersect here
if (IntersectSphere(origin, IOendp, ref IOintersect, p.m_center, p.m_radius))
{
// if it intersects,
if (RTUtils.IntersectPoly(p, origin, IOendp, ref IOintersect))
{
m_isectlst.Add(new ISectData(obj, p, IOintersect, origin, direction));
}
}
}
}
}
}
ISectData gp = ISectGroundPlane(direction, origin);
if (gp != null)
{
m_isectlst.Add(gp);
}
m_isectlst.Sort();
}
catch (Exception ex)
{
DebugLogger.Instance().LogError(ex.Message);
}
return m_isectlst;
}
// calc a split point normal from 2 edge normal and center tanget
Vector3d CalcCenterNormal(Vector3d norm1, Vector3d norm2, Vector3d tanget)
{
Vector3d tvec = (norm1 * 0.5f) + (norm2 * 0.5f);
tvec = Vector3d.cross(tvec, tanget);
Vector3d res = Vector3d.cross(tanget, tvec);
res.Normalize();
return(res);
}
public void MoveFromTip(ISectData dat)
{
Point3d center;
center = Centroid(); // get the centroid of the selected portion of the object
Vector3d isectnorm = new Vector3d();
//save the polygon intersection normal
isectnorm.x = dat.poly.m_normal.x;
isectnorm.y = dat.poly.m_normal.y;
isectnorm.z = dat.poly.m_normal.z;
MinMax mm = CalcMinMaxRange(s4i, m_lstpoints.Count);
float dist = (float)((mm.m_max - mm.m_min));
ResetTip();
Matrix3D tMat = new Matrix3D();
Vector3d vup = new Vector3d(0, 1, 0);
//always make sure the z is heading downward
if (isectnorm.z > 0)
{
isectnorm.z *= -1.0f;
}
// limit the z down to 45 degrees
if (isectnorm.z > -.75)
{
isectnorm.z = -.75f;
}
//reverse the direction to get the reflection
Vector3d dir = new Vector3d(-isectnorm.x, -isectnorm.y, -isectnorm.z);
dir.Normalize();
//create a matrix transform
tMat.LookAt(dir, vup);
Transform(tMat);
Translate(
(float)(dat.intersect.x),
(float)(dat.intersect.y),
(float)(dat.intersect.z));
//now, get the center of s4i to s5i
Point3d cnt = CalcCentroid(s4i, s5i);
//translate to 0
TranslateRange(-cnt.x, -cnt.y, -cnt.z, s4i, s5i);
//reset the points,
ReGenSegmentPoints(mSC.hbrad, mSC.cdivs, s4i, 0, false); // bottom of head
//move back
TranslateRange(cnt.x, cnt.y, cnt.z, s4i, s5i);
Update();
//
}
Vector3d GetTangetFromNormal(Vector3d norm, Vector3d dir)
{
Vector3d normxdir = Vector3d.cross(norm, dir);
Vector3d res = Vector3d.cross(normxdir, norm);
if ((Math.Abs(res.x) < Epsilon) && (Math.Abs(res.y) < Epsilon) && (Math.Abs(res.z) < Epsilon))
{
res = dir.clone();
}
res.Normalize();
//res = res * dir.Mag();
return(res);
}
Vector3d GetNormalFromTanget(Vector3d norm, Vector3d tanget)
{
Vector3d normtang = Vector3d.cross(norm, tanget);
Vector3d res = Vector3d.cross(tanget, normtang);
if ((Math.Abs(res.x) < Epsilon) && (Math.Abs(res.y) < Epsilon) && (Math.Abs(res.z) < Epsilon))
{
return(null);
}
res.Normalize();
//res = res * dir.Mag();
return(res);
}
// split edge v1-v2. v3 is the last corner in the triangle and is used for computing normals
/* int SplitEdge(int v1, int v2, int v3)
* {
* Vector3d norm1 = CalcNormal(v1, v2, v3);
* Vector3d norm2 = CalcNormal(v2, v3, v1);
* EdgeAmf edge = m_pointList[v1].FindEdge(v2);
* if (edge == null)
* {
* edge = m_pointList[v2].FindEdge(v1);
* if (edge != null)
* {
* // swap verteces tomatch edge
* int tv = v1;
* v1 = v2;
* v2 = tv;
* Vector3d tnorm = norm1;
* norm1 = norm2;
* norm2 = tnorm;
* }
* }
*
* Vector3d t1, t2;
* PointAmf pamf1 = m_pointList[v1];
* PointAmf pamf2 = m_pointList[v2];
* Point3d pt1 = pamf1.pt;
* Point3d pt2 = pamf2.pt;
* PointAmf pamf;
* float x, y, z;
*
* // calculate edge vector
* x = pt2.x - pt1.x;
* y = pt2.y - pt1.y;
* z = pt2.z - pt1.z;
* Vector3d edgeDir = new Vector3d(x, y, z);
*
* // first see if we have an edge for this segment
* if (edge != null)
* {
* // if this edge was already split, return result
* if (edge.v12 >= 0)
* return edge.v12;
* t1 = edge.t1;
* t2 = edge.t2;
* }
* /*else if ((pamf1.normal == null) && (pamf2.normal == null))
* {
* // its a linear line, return the center
* x = (pamf1.pt.x + pamf2.pt.x) / 2.0f;
* y = (pamf1.pt.y + pamf2.pt.y) / 2.0f;
* z = (pamf1.pt.z + pamf2.pt.z) / 2.0f;
* pamf = new PointAmf();
* pamf.pt = new Point3d(x, y, z);
* m_pointList.Add(pamf);
* return m_pointList.Count - 1;
* }
* else
* {
* // calculate tangets from normals.
* //edgeDir.Normalize();
* t1 = GetTangetFromNormal(norm1, edgeDir);
* t2 = GetTangetFromNormal(norm2, edgeDir);
* /*if (pamf1.normal == null)
* pamf1.normal = norm1;
* if (pamf2.normal == null)
* pamf2.normal = norm2;
* }
*
* float d = edgeDir.Mag();
*
* // calculate mid point using Hermite interpolation
* x = 0.5f * pt1.x + 0.125f * t1.x * d + 0.5f * pt2.x - 0.125f * t2.x * d;
* y = 0.5f * pt1.y + 0.125f * t1.y * d + 0.5f * pt2.y - 0.125f * t2.y * d;
* z = 0.5f * pt1.z + 0.125f * t1.z * d + 0.5f * pt2.z - 0.125f * t2.z * d;
*
* pamf = new PointAmf();
* pamf.pt = new Point3d(x, y, z);
* int v = m_pointList.Count;
* m_pointList.Add(pamf);
*
* // calculate new tanget and new normal
* x = -1.5f * pt1.x - 0.25f * t1.x * d + 1.5f * pt2.x - 0.25f * t2.x * d;
* y = -1.5f * pt1.y - 0.25f * t1.y * d + 1.5f * pt2.y - 0.25f * t2.y * d;
* z = -1.5f * pt1.z - 0.25f * t1.z * d + 1.5f * pt2.z - 0.25f * t2.z * d;
* Vector3d tanget = new Vector3d(x, y, z);
* tanget.Normalize();
*
* Vector3d tvec = (norm1 * 0.5f) + (norm2 * 0.5f);
* tvec = Vector3d.cross(tvec, tanget);
* pamf.normal = Vector3d.cross(tanget, tvec);
*
* if (edge == null)
* {
* //pamf.normal = GetNormalFromTanget(norm1, tanget);
* // create an edge for this segment
* edge = new EdgeAmf();
* edge.v1 = v1;
* edge.v2 = v2;
* edge.t1 = t1;
* edge.t2 = t2;
* pamf1.AddEdge(edge);
* }
* edge.v12 = m_pointList.Count - 1; // saves double computation
*
* //tanget.Normalize();
* // save 2 split edges
* EdgeAmf edge1 = new EdgeAmf();
* edge1.v1 = v1;
* edge1.v2 = v;
* edge1.t1 = t1;
* edge1.t2 = tanget;
* pamf1.AddEdge(edge1);
*
* EdgeAmf edge2 = new EdgeAmf();
* edge2.v1 = v;
* edge2.v2 = v2;
* edge2.t1 = tanget;
* edge2.t2 = t2;
* pamf.AddEdge(edge2);
*
* return v;
* }*/
// calc normal at corner v (looking at the triangle when corner v is at the bottom, v1 at top right, and v2 is at to left
Vector3d CalcNormal(int v, int v1, int v2)
{
PointAmf pamf = m_pointList[v];
if (pamf.normal != null)
{
return(pamf.normal);
}
Vector3d t1 = GetTanget(v, v1);
Vector3d t2 = GetTanget(v, v2);
Vector3d normal = Vector3d.cross(t1, t2);
normal.Normalize();
return(normal);
}
// create a look-at rotation matrix
public void LookAt(Vector3d dir, Vector3d up)
{
//Vector3d dir = new Vector3d(direction.x, direction.y, direction.z);
//dir.Normalize();
Vector3d vx = up.Cross(dir);
vx.Normalize();
Vector3d vy = dir.Cross(vx);
vy.Normalize();
Matrix[0, 0] = vx.x; Matrix[0, 1] = vx.y; Matrix[0, 2] = vx.z; Matrix[0, 3] = 0;
Matrix[1, 0] = vy.x; Matrix[1, 1] = vy.y; Matrix[1, 2] = vy.z; Matrix[1, 3] = 0;
Matrix[2, 0] = dir.x; Matrix[2, 1] = dir.y; Matrix[2, 2] = dir.z; Matrix[2, 3] = 0;
Matrix[3, 0] = 0; Matrix[3, 1] = 0; Matrix[3, 2] = 0; Matrix[3, 3] = 1;
}
/*
public class Config
{
int xres, yres;
// double
}
* */
public static bool FindIntersection(Vector3d direction, Point3d origin, ref Point3d intersect)
{
UVDLPApp.Instance().CalcScene();
//bool intersected = false;
// Point3d bpoint, tpoint;
// Point3d lowest = new Point3d(); // the lowest point of intersection on the z axis
direction.Normalize();
direction.Scale(100.0);
Point3d endp = new Point3d();
endp.Set(origin);
endp.x += direction.x;
endp.y += direction.y;
endp.z += direction.z;
/*
intersect = new Point3d();
intersect.x = 0.0d;
intersect.y = 0.0d;
intersect.z = 0.0d;
*/
//intersect the scene with a ray
// intersected = false;
foreach (Polygon p in UVDLPApp.Instance().Scene.m_lstpolys)
{
intersect = new Point3d();
// try a less- costly sphere intersect here
if (RTUtils.IntersectSphere(origin, endp, ref intersect, p.m_center, p.m_radius))
{
// if it intersects,
if (RTUtils.IntersectPoly(p, origin, endp, ref intersect))
{
return true;
/*
// and it's the lowest one
if (intersect.z <= lowest.z)
{
//save this point
intersected = true;
lowest.Set(intersect);
}
* */
}
}
}
return false;
}
public void ResetView(float x, float y, float z, float updeg, float lookz)
{
m_eye = new Vector3d(x, y, z);
m_lookat = new Vector3d(0, 0, 0);
m_up = new Vector3d(0, 0, 1);
m_dz = lookz;
m_dx = m_dy = 0;
//m_right = new Vector3d(1, 0, 0);
m_target = m_lookat - m_eye; // The "look-at" unit vector.
m_target.Normalize();
m_right = Vector3d.cross(m_target, m_up);
RotateUp(updeg);
UpdateView();
//Vector3d xaxis = Vector3d.cross(up, zaxis);// The "right" vector.
//xaxis.Normalize();
//Vector3d yaxis = Vector3d.cross(zaxis, xaxis); // The "up" vector.
}
public void MoveForward(float dist, float factor)
{
if (factor < 0.3)
{
factor = 0.3f;
}
dist = dist * factor;
Vector3d diff = (m_eye - m_lookat);
float len = Vector3d.length(diff) - dist;
if ((len <= 0) || (len >= 1000))
{
return;
}
diff.Normalize();
diff = diff * (float)dist;
m_eye = m_eye - diff;
UpdateView();
}
private static ISectData ISectObjSelPlane(Vector3d direction, Point3d origin)
{
ISectData isect = null;
if (m_selplane == null)
return null;
direction.Normalize();
direction.Scale(10000.0f);
ObSelendp.Set(origin);
ObSelendp.x += direction.x;
ObSelendp.y += direction.y;
ObSelendp.z += direction.z;
// intersect with the imaginary object selection plane
if (IntersectSphere(origin, ObSelendp, ref ObSelintersect, m_selplane.m_center, m_selplane.m_radius))
{
foreach (Polygon p in m_selplane.m_lstpolys)
{
// try a less- costly sphere intersect here
if (IntersectSphere(origin, ObSelendp, ref ObSelintersect, p.m_center, p.m_radius))
{
// if it intersects,
if (RTUtils.IntersectPoly(p, origin, ObSelendp, ref ObSelintersect))
{
isect = new ISectData(m_selplane, p, ObSelendp, origin, direction);
}
}
}
}
return isect;
}
public void RotateUp(float deg)
{
Rotate(m_right, deg);
// update target and up
m_target = m_lookat - m_eye; // The "look-at" unit vector.
m_target.Normalize();
m_up = Vector3d.cross(m_right, m_target);
UpdateView();
}
public void ResetView(float x, float y, float z, float updeg, float lookz)
{
m_eye = new Vector3d(x, y, z);
m_lookat = new Vector3d(0, 0, 0);
m_up = new Vector3d(0, 0, 1);
m_dz = lookz;
m_dx = m_dy = 0;
//m_right = new Vector3d(1, 0, 0);
m_target = m_lookat - m_eye; // The "look-at" unit vector.
m_target.Normalize();
m_right = Vector3d.cross(m_target, m_up);
RotateUp(updeg);
UpdateView();
//Vector3d xaxis = Vector3d.cross(up, zaxis);// The "right" vector.
//xaxis.Normalize();
//Vector3d yaxis = Vector3d.cross(zaxis, xaxis); // The "up" vector.
}
public void MoveFromTip(ISectData dat)
{
Point3d center;
center = Centroid(); // get the centroid of the selected portion of the object
Vector3d isectnorm = new Vector3d();
//save the polygon intersection normal
isectnorm.x = dat.poly.m_normal.x;
isectnorm.y = dat.poly.m_normal.y;
isectnorm.z = dat.poly.m_normal.z;
MinMax mm = CalcMinMaxRange(s4i, m_lstpoints.Count);
float dist = (float)((mm.m_max - mm.m_min) );
ResetTip();
Matrix3D tMat = new Matrix3D();
Vector3d vup = new Vector3d(0,1,0);
//always make sure the z is heading downward
if (isectnorm.z > 0)
{
isectnorm.z *= -1.0f;
}
// limit the z down to 45 degrees
if (isectnorm.z > -.75)
{
isectnorm.z = -.75f;
}
//reverse the direction to get the reflection
Vector3d dir = new Vector3d(-isectnorm.x, -isectnorm.y, -isectnorm.z);
dir.Normalize();
//create a matrix transform
tMat.LookAt(dir, vup);
Transform(tMat);
Translate(
(float)(dat.intersect.x),
(float)(dat.intersect.y),
(float)(dat.intersect.z));
//now, get the center of s4i to s5i
Point3d cnt = CalcCentroid(s4i, s5i);
//translate to 0
TranslateRange(-cnt.x, -cnt.y, -cnt.z, s4i, s5i);
//reset the points,
ReGenSegmentPoints(mSC.hbrad, mSC.cdivs, s4i, 0, false); // bottom of head
//move back
TranslateRange(cnt.x, cnt.y, cnt.z, s4i, s5i);
Update();
//
}
/// <summary>
/// This functiuon positions the base at the location 'intersect'
/// The idir is the direction that was used to intersect
/// </summary>
/// <param name="intersect"></param>
/// <param name="idir"></param>
/// <param name="inorm"></param>
public void PositionBottom(ISectData dat)
{
// the bottom could be a tip or base
// need to orient the bottom and position it.
Point3d center;
// for a base support, just slide it around
if (m_subtype == eSubType.eBase)
{
center = Centroid(); // get the centroid of the selected portion of the object
MinMax mm = CalcMinMaxRange(s1i, s4i);
float dist = (float)((mm.m_max - mm.m_min) / 2);
Translate(
(float)(dat.intersect.x - center.x),
(float)(dat.intersect.y - center.y),
(float)(dat.intersect.z - center.z + dist));
}
else if (m_subtype == eSubType.eIntra) // bottom tip
{
// for a base tip, find the angle and angle it in
Vector3d isectnorm = new Vector3d();
//save the polygon intersection normal
isectnorm.x = dat.poly.m_normal.x;
isectnorm.y = dat.poly.m_normal.y;
isectnorm.z = dat.poly.m_normal.z;
isectnorm.Normalize();
if (isectnorm.z < 0)
{
isectnorm.z *= -1.0f;
}
// limit the z down to 45 degrees
if (isectnorm.z < .75)
{
isectnorm.z = .75f;
}
// re-genrate the points on the bottom of the foot
ReGenSegmentPoints(mSC.htrad, mSC.cdivs, s1i, 0, true); // bottom of foot is the tip radius
Matrix3D tMat = new Matrix3D();
Vector3d vup = new Vector3d(0, 1, 0);
Vector3d dir = new Vector3d(isectnorm.x, isectnorm.y, isectnorm.z);
dir.Normalize();
//direction should be upward at this point.
//create a matrix transform
tMat.LookAt(dir, vup);
//transform the si1-s2i points to look at the vector
TransformRange(tMat, s1i, s2i);
//move the range of points to be touching the intersection point
STranslateRange(dat.intersect.x, dat.intersect.y, dat.intersect.z, s1i, s2i);
//now, get the center of s4i to s5i
Point3d cnt = CalcCentroid(s2i, s4i);
//translate to 0,0,0
STranslateRange(-cnt.x, -cnt.y, -cnt.z, s2i, s4i);
//reset the points,
ReGenSegmentPoints(mSC.hbrad, mSC.cdivs, s2i, 0, false); // top of foot
ReGenSegmentPoints(mSC.hbrad, mSC.cdivs, s3i, 0, false); // top of foot
Point3d newp = new Point3d();
newp.x = dat.intersect.x + (isectnorm.x * 2);
newp.y = dat.intersect.y + (isectnorm.y * 2);
newp.z = dat.intersect.z + (isectnorm.z * 2);
STranslateRange(newp.x,newp.y,newp.z, s2i, s4i);
Update();
}
//Update();
}
public static bool IntersectSphere(Point3d start,Point3d end,ref Point3d intersect, Point3d center,double radius)
{
bool retval = false;
double EO;//EO is distance from start of ray to center of sphere
double d,disc,v;//v is length of direction ray
Vector3d V,temp;//V is unit vector of the ray
temp =new Vector3d();
V = new Vector3d();
temp.Set(center.x - start.x,center.y - start.y, center.z - start.z,0);
EO = temp.Mag(); // unnormalized length
V.Set(end.x - start.x,end.y - start.y,end.z - start.z,0);
v = V.Mag();// magnitude of direction vector
V.Normalize();// normalize the direction vector
disc = (radius*radius) - ((EO*EO) - (v*v));
if(disc < 0.0f)
{
retval = false;// no intersection
}
else
{ // compute the intersection point
retval = true;
d = Math.Sqrt(disc);
intersect.x = start.x + ((v-d)*V.x);
intersect.y = start.y + ((v-d)*V.y);
intersect.z = start.z + ((v-d)*V.z);
}
return retval;
}
/// <summary>
/// For now this is the editing mode for the currently selected support
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void glControl1_Click(object sender, EventArgs e)
{
// single click on GL Control
Object3d obj = UVDLPApp.Instance().SelectedObject;
if (obj == null) return;
if (ctrldown == false) return; // ctrl need to be held down
// this object is a support
if (obj.tag == Object3d.OBJ_SUPPORT)
{
Support sup = (Support)obj;// we can cast safely
// now we have to see if we clicked on an object
MouseEventArgs me = e as MouseEventArgs;
MouseButtons buttonPushed = me.Button;
int xPos = me.X;
int yPos = me.Y;
List<ISectData> isects = TestHitTest(xPos, yPos);
if (isects.Count == 0) return; // no intersections
ISectData isd1 =null;
foreach (ISectData isd in isects)
{
// find the closest object we clicked
if (isd.obj.tag == Object3d.OBJ_NORMAL)
{
isd1 = isd; // save it
break;
}
}
if (isd1 == null) return; // no object intersection
isd1.poly.CalcNormal();
m_isectnormal.x = isd1.poly.m_normal.x;
m_isectnormal.y = isd1.poly.m_normal.y;
m_isectnormal.z = isd1.poly.m_normal.z;
// ok, we've got the normal, we know where we've intersected
// my best guess is that we should move the support 5mm in the direction of the camera
// the tip of the support should touch the intersection point
// let's start with scaling the height...
//sup.ScaleToHeight(isd1.intersect.z);
//m_camera.m_eye
Engine3D.Vector3d towardseye = new Engine3D.Vector3d();
towardseye = m_isectnormal;// -m_camera.m_eye;
towardseye.Normalize(); // make the unit length of 1
towardseye.Scale(4.0f); // scale to 5 mm
sup.MoveFromTip(isd1.intersect, towardseye);
UpdateView();
//sup.
}
}
/// <summary>
/// This functiuon positions the base at the location 'intersect'
/// The idir is the direction that was used to intersect
/// </summary>
/// <param name="intersect"></param>
/// <param name="idir"></param>
/// <param name="inorm"></param>
public void PositionBottom(ISectData dat)
{
// the bottom could be a tip or base
// need to orient the bottom and position it.
Point3d center;
// for a base support, just slide it around
if (m_subtype == eSubType.eBase)
{
center = Centroid(); // get the centroid of the selected portion of the object
MinMax mm = CalcMinMaxRange(s1i, s4i);
float dist = (float)((mm.m_max - mm.m_min) / 2);
Translate(
(float)(dat.intersect.x - center.x),
(float)(dat.intersect.y - center.y),
(float)(dat.intersect.z - center.z + dist));
}
else if (m_subtype == eSubType.eIntra) // bottom tip
{
// for a base tip, find the angle and angle it in
Vector3d isectnorm = new Vector3d();
//save the polygon intersection normal
isectnorm.x = dat.poly.m_normal.x;
isectnorm.y = dat.poly.m_normal.y;
isectnorm.z = dat.poly.m_normal.z;
isectnorm.Normalize();
if (isectnorm.z < 0)
{
isectnorm.z *= -1.0f;
}
// limit the z down to 45 degrees
if (isectnorm.z < .75)
{
isectnorm.z = .75f;
}
// re-genrate the points on the bottom of the foot
ReGenSegmentPoints(mSC.htrad, mSC.cdivs, s1i, 0, true); // bottom of foot is the tip radius
Matrix3D tMat = new Matrix3D();
Vector3d vup = new Vector3d(0, 1, 0);
Vector3d dir = new Vector3d(isectnorm.x, isectnorm.y, isectnorm.z);
dir.Normalize();
//direction should be upward at this point.
//create a matrix transform
tMat.LookAt(dir, vup);
//transform the si1-s2i points to look at the vector
TransformRange(tMat, s1i, s2i);
//move the range of points to be touching the intersection point
STranslateRange(dat.intersect.x, dat.intersect.y, dat.intersect.z, s1i, s2i);
//now, get the center of s4i to s5i
Point3d cnt = CalcCentroid(s2i, s4i);
//translate to 0,0,0
STranslateRange(-cnt.x, -cnt.y, -cnt.z, s2i, s4i);
//reset the points,
ReGenSegmentPoints(mSC.hbrad, mSC.cdivs, s2i, 0, false); // top of foot
ReGenSegmentPoints(mSC.hbrad, mSC.cdivs, s3i, 0, false); // top of foot
Point3d newp = new Point3d();
newp.x = dat.intersect.x + (isectnorm.x * 2);
newp.y = dat.intersect.y + (isectnorm.y * 2);
newp.z = dat.intersect.z + (isectnorm.z * 2);
STranslateRange(newp.x, newp.y, newp.z, s2i, s4i);
Update();
}
//Update();
}
// split edge v1-v2. v3 is the last corner in the triangle and is used for computing normals
int SplitEdge(int v1, int v2, Vector3d norm1, Vector3d norm2, out Vector3d norm12)
{
EdgeAmf edge = m_pointList[v1].FindEdge(v2);
if (edge == null)
{
edge = m_pointList[v2].FindEdge(v1);
if (edge != null)
{
// swap verteces to match edge
int tv = v1;
v1 = v2;
v2 = tv;
Vector3d tnorm = norm1;
norm1 = norm2;
norm2 = tnorm;
}
}
Vector3d t1, t2;
PointAmf pamf1 = m_pointList[v1];
PointAmf pamf2 = m_pointList[v2];
Point3d pt1 = pamf1.pt;
Point3d pt2 = pamf2.pt;
PointAmf pamf;
float x, y, z;
// calculate edge vector
x = pt2.x - pt1.x;
y = pt2.y - pt1.y;
z = pt2.z - pt1.z;
Vector3d edgeDir = new Vector3d(x, y, z);
// first see if we have an edge for this segment
if (edge != null)
{
// if this edge was already split, return result
if (edge.v12 >= 0)
{
norm12 = CalcCenterNormal(norm1, norm2, edge.t12);
return(edge.v12);
}
t1 = edge.t1;
t2 = edge.t2;
}
else
{
t1 = GetTangetFromNormal(norm1, edgeDir);
t2 = GetTangetFromNormal(norm2, edgeDir);
}
float d = edgeDir.Mag();
// calculate mid point using Hermite interpolation
x = 0.5f * pt1.x + 0.125f * t1.x * d + 0.5f * pt2.x - 0.125f * t2.x * d;
y = 0.5f * pt1.y + 0.125f * t1.y * d + 0.5f * pt2.y - 0.125f * t2.y * d;
z = 0.5f * pt1.z + 0.125f * t1.z * d + 0.5f * pt2.z - 0.125f * t2.z * d;
pamf = new PointAmf();
pamf.pt = new Point3d(x, y, z);
int v = m_pointList.Count;
m_pointList.Add(pamf);
// calculate new tanget and new normal
x = -1.5f * pt1.x - 0.25f * t1.x * d + 1.5f * pt2.x - 0.25f * t2.x * d;
y = -1.5f * pt1.y - 0.25f * t1.y * d + 1.5f * pt2.y - 0.25f * t2.y * d;
z = -1.5f * pt1.z - 0.25f * t1.z * d + 1.5f * pt2.z - 0.25f * t2.z * d;
Vector3d tanget = new Vector3d(x, y, z);
tanget.Normalize();
norm12 = CalcCenterNormal(norm1, norm2, tanget);
if (edge == null)
{
//pamf.normal = GetNormalFromTanget(norm1, tanget);
// create an edge for this segment
edge = new EdgeAmf();
edge.v1 = v1;
edge.v2 = v2;
edge.t1 = t1;
edge.t2 = t2;
pamf1.AddEdge(edge);
}
edge.t12 = tanget;
edge.v12 = m_pointList.Count - 1; // saves double computation
//tanget.Normalize();
// save 2 split edges
EdgeAmf edge1 = new EdgeAmf();
edge1.v1 = v1;
edge1.v2 = v;
edge1.t1 = t1;
edge1.t2 = tanget;
pamf1.AddEdge(edge1);
EdgeAmf edge2 = new EdgeAmf();
edge2.v1 = v;
edge2.v2 = v2;
edge2.t1 = tanget;
edge2.t2 = t2;
pamf.AddEdge(edge2);
return(v);
}
private static ISectData ISectGroundPlane(Vector3d direction, Point3d origin)
{
ISectData isect = null;
direction.Normalize();
direction.Scale(10000.0f);
GPendp.Set(origin);
GPendp.x += direction.x;
GPendp.y += direction.y;
GPendp.z += direction.z;
// intersect with the imaginary groundplane object;
if (m_gp == null)
{
CreateGroundPlane();
}
if (IntersectSphere(origin, GPendp, ref GPintersect, m_gp.m_center, m_gp.m_radius))
{
foreach (Polygon p in m_gp.m_lstpolys)
{
//GPintersect = new Point3d();
// try a less- costly sphere intersect here
if (IntersectSphere(origin, GPendp, ref GPintersect, p.m_center, p.m_radius))
{
// if it intersects,
if (RTUtils.IntersectPoly(p, origin, GPendp, ref GPintersect))
{
isect = new ISectData(m_gp, p, GPintersect, origin, direction);
}
}
}
}
return isect;
}
// split edge v1-v2. v3 is the last corner in the triangle and is used for computing normals
int SplitEdge(int v1, int v2, Vector3d norm1, Vector3d norm2, out Vector3d norm12)
{
EdgeAmf edge = m_pointList[v1].FindEdge(v2);
if (edge == null)
{
edge = m_pointList[v2].FindEdge(v1);
if (edge != null)
{
// swap verteces to match edge
int tv = v1;
v1 = v2;
v2 = tv;
Vector3d tnorm = norm1;
norm1 = norm2;
norm2 = tnorm;
}
}
Vector3d t1, t2;
PointAmf pamf1 = m_pointList[v1];
PointAmf pamf2 = m_pointList[v2];
Point3d pt1 = pamf1.pt;
Point3d pt2 = pamf2.pt;
PointAmf pamf;
float x, y, z;
// calculate edge vector
x = pt2.x - pt1.x;
y = pt2.y - pt1.y;
z = pt2.z - pt1.z;
Vector3d edgeDir = new Vector3d(x, y, z);
// first see if we have an edge for this segment
if (edge != null)
{
// if this edge was already split, return result
if (edge.v12 >= 0)
{
norm12 = CalcCenterNormal(norm1, norm2, edge.t12);
return edge.v12;
}
t1 = edge.t1;
t2 = edge.t2;
}
else
{
t1 = GetTangetFromNormal(norm1, edgeDir);
t2 = GetTangetFromNormal(norm2, edgeDir);
}
float d = edgeDir.Mag();
// calculate mid point using Hermite interpolation
x = 0.5f * pt1.x + 0.125f * t1.x * d + 0.5f * pt2.x - 0.125f * t2.x * d;
y = 0.5f * pt1.y + 0.125f * t1.y * d + 0.5f * pt2.y - 0.125f * t2.y * d;
z = 0.5f * pt1.z + 0.125f * t1.z * d + 0.5f * pt2.z - 0.125f * t2.z * d;
pamf = new PointAmf();
pamf.pt = new Point3d(x, y, z);
int v = m_pointList.Count;
m_pointList.Add(pamf);
// calculate new tanget and new normal
x = -1.5f * pt1.x - 0.25f * t1.x * d + 1.5f * pt2.x - 0.25f * t2.x * d;
y = -1.5f * pt1.y - 0.25f * t1.y * d + 1.5f * pt2.y - 0.25f * t2.y * d;
z = -1.5f * pt1.z - 0.25f * t1.z * d + 1.5f * pt2.z - 0.25f * t2.z * d;
Vector3d tanget = new Vector3d(x, y, z);
tanget.Normalize();
norm12 = CalcCenterNormal(norm1, norm2, tanget);
if (edge == null)
{
//pamf.normal = GetNormalFromTanget(norm1, tanget);
// create an edge for this segment
edge = new EdgeAmf();
edge.v1 = v1;
edge.v2 = v2;
edge.t1 = t1;
edge.t2 = t2;
pamf1.AddEdge(edge);
}
edge.t12 = tanget;
edge.v12 = m_pointList.Count - 1; // saves double computation
//tanget.Normalize();
// save 2 split edges
EdgeAmf edge1 = new EdgeAmf();
edge1.v1 = v1;
edge1.v2 = v;
edge1.t1 = t1;
edge1.t2 = tanget;
pamf1.AddEdge(edge1);
EdgeAmf edge2 = new EdgeAmf();
edge2.v1 = v;
edge2.v2 = v2;
edge2.t1 = tanget;
edge2.t2 = t2;
pamf.AddEdge(edge2);
return v;
}
