bool lmRayTriangleIntersection(const lmVectorND<T,N> &RayPoint,const lmVectorND<T,N> &RayDirection,const lmVectorND<T,N> &PlaneP1,const lmVectorND<T,N> &PlaneP2,const lmVectorND<T,N> &PlaneP3,const lmVectorND<T,N> &PlaneNormal,T *iDist = nullptr,lmVectorND<T,N> *iPoint = nullptr)
{
T IntersectionDist = 0;
lmVectorND<T,N> IntersectionPoint(lmVectorND<T,N>::NO_INIT);
if(lmRayPlaneIntersection(RayPoint,RayDirection,PlaneP1,PlaneNormal,&IntersectionDist,&IntersectionPoint))
{
T cTriSurface = lmHeronFormula(PlaneP1,PlaneP2,PlaneP3);
T Tri1Surface = lmHeronFormula(PlaneP1,PlaneP2,IntersectionPoint);
T Tri2Surface = lmHeronFormula(IntersectionPoint,PlaneP2,PlaneP3);
T Tri3Surface = lmHeronFormula(PlaneP1,IntersectionPoint,PlaneP3);
T SumTriSurface = Tri1Surface + Tri2Surface + Tri3Surface;
if(abs(cTriSurface - SumTriSurface) < 1e-3)
{
if(iDist != nullptr)
{*iDist = IntersectionDist;}
if(iPoint != nullptr)
{*iPoint = IntersectionPoint;}
return true;
}
}
if(iDist != nullptr)
{*iDist = 0;}
if(iPoint != nullptr)
{*iPoint = lmVectorND<T,N>(lmVectorND<T,N>::NO_INIT);}
return false;
}
void TriKickCollisionResponse::Response(PhysObjDescriptor* object, CollisionModel3D *CollisionModel, bool ismodelowner)
{
//CollisionModel3D *OwnCollisionModel = Owner->GetOwner()->GetCollidableModule()->GetCollisionModel();
float t1[9], t2[9], p[3], *t;
CollisionModel->getCollisionPoint(p,false);
Ogre::Vector3 IntersectionPoint(p[0],p[1],p[2]);
CollisionModel->getCollidingTriangles(t1, t2, false);
if (ismodelowner)
t=t1;
else
t=t2;
Ogre::Plane collidplane(Ogre::Vector3(t[0],t[1],t[2]),
Ogre::Vector3(t[3],t[4],t[5]),
Ogre::Vector3(t[6],t[7],t[8]));
double own_mass = Owner->GetMass(), obj_mass = object->Object->GetMass();
Ogre::Vector3 linvel = object->LinVelocity;
Ogre::Vector3 Normal = collidplane.normal;
Ogre::Vector3 shift = (object->Object->GetPosition()-Owner->GetPosition())*own_mass/obj_mass;
AAUtilities::Clamp(shift, 5000);
/*char log[100];
sprintf(log,"1 x:%f y:%f z:%f\n", shift.x, shift.y, shift.z);
Debugging::Log("tkcr",log);*/
Ogre::Vector3 vretreat = shift;
vretreat.normalise();
vretreat = vretreat*Owner->GetLinVelocity();
Ogre::Vector3 throttle = object->Throttle;
object->Object->AddCollisionImpulse(IntersectionPoint, (Normal+vretreat)*Owner->GetMass()/(object->Object->GetMass()), &Owner->GetDescriptor()); //*object->GetMass()/10
/*if (!throttle.isZeroLength())
object->Throttle = -throttle;
else
object->Object->AddForce(IntersectionPoint, (Normal+vretreat)*Owner->GetMass()/object->Object->GetMass()); //*object->GetMass()/2
*/
vretreat = Normal + shift;
//vretreat = Normal + vretreat;
object->VelocityVector = vretreat;
object->LinVelocity = vretreat;
object->Throttle = Ogre::Vector3::ZERO;
object->LinVelocityAccel = vretreat;
object->Object->SetForces(Ogre::Vector3::ZERO);
object->Collided = true; //Descriptor->Collided=true;
}
//==========================================================================*
// Intersection point (Schnittpunkt zweier Geraden)
//--------------------------------------------------------------------------*
bool IntersectionPoint2D(
const TVec3d& p0,
const TVec3d& v0,
const TVec3d& p1,
const TVec3d& v1,
double& t )
{
return IntersectionPoint(p0.x, p0.y, v0.x, v0.y, p1.x, p1.y, v1.x, v1.y, t);
}
bool IntersectedPolygon(Vector3 vPoly[], Vector3 vLine[], int verticeCount)
{
Vector3 vNormal;
float originDistance = 0;
if(!IntersectedPlane(vPoly, vLine,vNormal,originDistance))
return false;
Vector3 vIntersection = IntersectionPoint(vNormal, vLine, originDistance);//计算交点
if(InsidePolygon(vIntersection, vPoly, verticeCount))//找到交点，判断是否在多边形内
return true;
return false;
}
bool IntersectedPolygon(CVector3 vPoly[], CVector3 vLine[], int verticeCount)
{
CVector3 vNormal;
float originDistance = 0;
// First, make sure our line intersects the plane
// Reference // Reference
if(!IntersectedPlane(vPoly, vLine, vNormal, originDistance))
return false;
// Now that we have our normal and distance passed back from IntersectedPlane(),
// we can use it to calculate the intersection point.
CVector3 vIntersection = IntersectionPoint(vNormal, vLine, originDistance);
// Now that we have the intersection point, we need to test if it's inside the polygon.
if(InsidePolygon(vIntersection, vPoly, verticeCount))
return true; // We collided! Return success
return false; // There was no collision, so return false
}
void SlideCollisionResponse::Response(PhysObjDescriptor* object, CollisionModel3D *CollisionModel, bool ismodelowner)
{
// при коллизии определяется треугольник и объект движется параллельно плоскости тругольника
float t1[9], t2[9], p[3], *t;
CollisionModel->getCollisionPoint(p,false);
Ogre::Vector3 normal, IntersectionPoint(p[0],p[1],p[2]);
CollisionModel->getCollidingTriangles(t1, t2, false);
if (ismodelowner)
t=t1;
else
t=t2;
Ogre::Plane collidplane(Ogre::Vector3(t[0],t[1],t[2]),
Ogre::Vector3(t[3],t[4],t[5]),
Ogre::Vector3(t[6],t[7],t[8]));
Ogre::Vector3 proj = collidplane.projectVector(object->LinVelocity);
proj.normalise();
Ogre::Vector3 Normal = collidplane.normal;
Ogre::Vector3 linvel = object->LinVelocity;
Ogre::Vector3 throttle = object->Throttle;
Ogre::Vector3 dir=proj+Normal/2;
dir.normalise();
object->Object->SetForces(Ogre::Vector3::ZERO);
object->VelocityVector = dir*(linvel.length()+1);
object->LinVelocity = dir*(linvel.length()+1);
object->Object->SetReplacingDirection(dir*(linvel.length()+1));
object->Object->AddForce(IntersectionPoint, dir*(linvel.length()+1)*Owner->GetMass()/object->Object->GetMass());
}
bool lmSphereTriangleIntersection(const lmVectorND<T,N> &SphereCenter,const T SphereRadius,const lmVectorND<T,N> &PlaneP1,const lmVectorND<T,N> &PlaneP2,const lmVectorND<T,N> &PlaneP3,const lmVectorND<T,N> &PlaneNormal,lmScalar *iDist = nullptr,lmVectorND<T,N> *iPoint = nullptr)
{
T IntersectionDist = 0;
lmVectorND<T,N> IntersectionPoint(lmVectorND<T,N>::NO_INIT);
if(lmRayTriangleIntersection(SphereCenter,-1*PlaneNormal,PlaneP1,PlaneP2,PlaneP3,PlaneNormal,&IntersectionDist,&IntersectionPoint) && (abs(IntersectionDist) <= SphereRadius))
{
if(iDist != nullptr)
{*iDist = IntersectionDist;}
if(iPoint != nullptr)
{*iPoint = IntersectionPoint;}
return true;
}
if(iDist != nullptr)
{*iDist = 0;}
if(iPoint != nullptr)
{*iPoint = lmVectorND<T,N>(lmVectorND<T,N>::NO_INIT);}
return false;
}
bool solve()
{
if(IsOnlyOnePoint()) return true;
memset(del, false, sizeof(del));
for(int i = 0; i < n; ++i)
for(int j = 0; j < n; ++j)
{
if(del[j] || i == j) continue;
if(cir[i].contain(cir[j]))
{
del[i] = true;
break;
}
}
double ans = 0.0;
for(int i = 0; i < n; ++i)
{
if(del[i]) continue;
last->clear();
Point p1, p2;
for(int j = 0; j < n; ++j)
{
if(del[j] || i == j) continue;
if(!cir[i].intersect(cir[j])) return false;
cur->clear();
IntersectionPoint(cir[i], cir[j], p1, p2);
double rs = Angle(p2 - cir[i].c);
double rt = Angle(p1 - cir[i].c);
if(dcmp(rs) < 0) rs += 2 * PI;
if(dcmp(rt) < 0) rt += 2 * PI;
if(last->n == 0)
{
if(dcmp(rt - rs) < 0)
{
cur->add(Region(rs, 2*PI));
cur->add(Region(0, rt));
}
else cur->add(Region(rs, rt));
}
else
{
for(int k = 0; k < last->n; ++k)
{
if(dcmp(rt - rs) < 0)
{
if(dcmp(last->v[k].st-rt) >= 0 && dcmp(last->v[k].ed-rs) <= 0) continue;
if(dcmp(last->v[k].st-rt) < 0) cur->add(Region(last->v[k].st, std::min(last->v[k].ed, rt)));
if(dcmp(last->v[k].ed-rs) > 0) cur->add(Region(std::max(last->v[k].st, rs), last->v[k].ed));
}
else
{
if(dcmp(rt-last->v[k].st <= 0 || dcmp(rs-last->v[k].ed) >= 0)) continue;
cur->add(Region(std::max(rs, last->v[k].st), std::min(rt, last->v[k].ed)));
}
}
}
std::swap(cur, last);
if(last->n == 0) break;
}
for(int j = 0; j < last->n; ++j)
{
p1 = cir[i].get_point(last->v[j].st);
p2 = cir[i].get_point(last->v[j].ed);
ans += Cross(p1, p2) / 2;
double ang = last->v[j].ed - last->v[j].st;
ans += cir[i].r * cir[i].r * (ang - sin(ang)) / 2;
}
}
if(dcmp(ans) == 0) return false;
printf("The total possible area is %.2f.\n", ans);
return true;
}
vec3 EditorToolBar::getGroundPickPos(float elevation) const
{
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
g_Camera.setCamera();
vec3 groundPos;
groundPos.zero();
// Get the point on the ground plane that the mouse is hovering over.
// Get a ray for the mouse
vec3 mouseRay[] =
{
UnProject(0.0f),
UnProject(1.0f),
};
// Get the location that the mouse ray intersect the ground plane at.
// Check the mouse ray against the ground plane (XZ plane)
float originDistance=0.0f;
vec3 normal;
// A polygon on the intersection plane
vec3 a(0,elevation,0);
vec3 b(0,elevation,1);
vec3 c(1,elevation,0);
vec3 vPoly[3] = {a, b, c};
if(IntersectedPlane(vPoly, mouseRay, normal, originDistance) == true)
{
// Get the intersection point
groundPos = IntersectionPoint(normal, mouseRay, (double)originDistance);
// Snap to grid
if(snapToGrid)
{
float snap = 0.25f;
float snappedX = (snap) * floorf(groundPos.x / snap);
float snappedZ = (snap) * floorf(groundPos.z / snap);
groundPos.x = snappedX;
groundPos.z = snappedZ;
}
}
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
return groundPos;
}
void Frustum::Define()
{
const float* m = GetPointer(m_);
float t;
Vector4& left = planes_[PLANE_LEFT].GetNormalDRef();
left[0] = m[3] + m[0];
left[1] = m[7] + m[4];
left[2] = m[11] + m[8];
left[3] = m[15] + m[12];
t = Length(Vector3(left));
left /= t;
Vector4& right = planes_[PLANE_RIGHT].GetNormalDRef();
right[0] = m[3] - m[0];
right[1] = m[7] - m[4];
right[2] = m[11] - m[8];
right[3] = m[15] - m[12];
t = Length(Vector3(right));
right /= t;
Vector4& down = planes_[PLANE_DOWN].GetNormalDRef();
down[0] = m[3] + m[1];
down[1] = m[7] + m[5];
down[2] = m[11] + m[9];
down[3] = m[15] + m[13];
t = Length(Vector3(down));
down /= t;
Vector4& up = planes_[PLANE_UP].GetNormalDRef();
up[0] = m[3] - m[1];
up[1] = m[7] - m[5];
up[2] = m[11] - m[9];
up[3] = m[15] - m[13];
t = Length(Vector3(up));
up /= t;
Vector4& nearP = planes_[PLANE_NEAR].GetNormalDRef();
nearP[0] = m[3] + m[2];
nearP[1] = m[7] + m[6];
nearP[2] = m[11] + m[10];
nearP[3] = m[15] + m[14];
t = Length(Vector3(nearP));
nearP /= t;
Vector4& farP = planes_[PLANE_FAR].GetNormalDRef();
farP[0] = m[3] - m[2];
farP[1] = m[7] - m[6];
farP[2] = m[11] - m[10];
farP[3] = m[15] - m[14];
t = Length(Vector3(farP));
farP /= t;
const Plane& Near = planes_[PLANE_NEAR];
const Plane& Far = planes_[PLANE_FAR];
const Plane& Left = planes_[PLANE_LEFT];
const Plane& Right = planes_[PLANE_RIGHT];
const Plane& Top = planes_[PLANE_UP];
const Plane& Bottom = planes_[PLANE_DOWN];
vertices_[0] = IntersectionPoint(Near, Right, Top);
vertices_[1] = IntersectionPoint(Near, Right, Bottom);
vertices_[2] = IntersectionPoint(Near, Left, Bottom);
vertices_[3] = IntersectionPoint(Near, Left, Top);
vertices_[4] = IntersectionPoint(Far, Right, Top);
vertices_[5] = IntersectionPoint(Far, Right, Bottom);
vertices_[6] = IntersectionPoint(Far, Left, Bottom);
vertices_[7] = IntersectionPoint(Far, Left, Top);
BuildFaces();
origin_.x = vertices_[2].x + 0.5f * (vertices_[1].x - vertices_[2].x);
origin_.y = vertices_[2].y + 0.5f * (vertices_[3].y - vertices_[2].y);
origin_.z = vertices_[2].z;
}
Intersection(IntersectionPoint left_ = IntersectionPoint(), IntersectionPoint right_ = IntersectionPoint()):
left(left_), right(right_) {}
