C++ btVector3::distance2方法代码示例

        virtual btScalar addSingleResult(btManifoldPoint& cp,
                                         const btCollisionObject* col0, int partId0, int index0,
                                         const btCollisionObject* col1, int partId1, int index1)
        {
            const RigidBody* body = dynamic_cast<const RigidBody*>(col1);
            if(body && body->mName != mFilter)
            {
                btScalar distsqr = mOrigin.distance2(cp.getPositionWorldOnA());
                if(!mObject || distsqr < mLeastDistSqr)
                {
                    mObject = body;
                    mLeastDistSqr = distsqr;
                    mContactPoint = cp.getPositionWorldOnA();
                }
            }

            return 0.f;
        }

// -----------------------------------------------------------------------------
void IrrDebugDrawer::drawLine(const btVector3& from, const btVector3& to,
                              const btVector3& color)
{
    video::SColor c(255, (int)(color.getX()*255), (int)(color.getY()*255),
                         (int)(color.getZ()*255)                          );

    //World::getWorld()->getCa

    if (from.distance2(m_camera_pos) > 10000) return;

    std::vector<float>& v = m_lines[c];
    v.push_back(from.getX());
    v.push_back(from.getY());
    v.push_back(from.getZ());
    v.push_back(to.getX());
    v.push_back(to.getY());
    v.push_back(to.getZ());
    //draw3DLine((const core::vector3df&)from, (const core::vector3df&)to, c);
}

btScalar getObjectMaximumGravityTorqueLength(const btCollisionShape &object_shape)
{
	// Calculate the maximum acceleration that can happen for a particular object under gravity
	std::vector<btVector3> object_corner_point_list;
	object_corner_point_list.reserve(150);

	// get all points in the compound object
	if (object_shape.isCompound())
	{
		const btCompoundShape * object_compound_shape = (btCompoundShape *)&object_shape;

		for (int i = 0; i < object_compound_shape->getNumChildShapes(); i++)
		{
			const btCollisionShape * child_shape = object_compound_shape->getChildShape(i);
			const btTransform child_transform = object_compound_shape->getChildTransform(i);
			if (child_shape->isConvex())
			{
				const btConvexHullShape * child_hull_shape = (btConvexHullShape *)child_shape;
				for (int pts = 0; pts < child_hull_shape->getNumPoints(); pts++)
				{
					btVector3 corner_points = child_hull_shape->getScaledPoint(pts);
					object_corner_point_list.push_back(child_transform * corner_points);
				}
			}
		}
	}

	// get the farthest distance from the object center of gravity
	// const btVector3 cog = object.getCenterOfMassPosition();
	const btVector3 cog(0.,0.,0.);
	btScalar max_dist_squared = 0;
	for (std::vector<btVector3>::iterator it = object_corner_point_list.begin(); it!= object_corner_point_list.end(); ++it)
	{
		btScalar point_distance_to_cog =  cog.distance2(*it);
		max_dist_squared = (point_distance_to_cog > max_dist_squared) ? point_distance_to_cog : max_dist_squared;
	}

	return sqrt(max_dist_squared);
}

static bool IsEqual(const btVector3 &pt0, const btVector3 &pt1) {
	return pt0.distance2(pt1) < 1e-8f;
}

// clips an obstacle shape if it is within range of the sensors
// if not returns false
bool cSpace::clipShape(btVector3 cc, QList<btVector3>& ls)
{
	if(ls.size() < 2) return false;
	int j=0;
	cc.setZ(0);
	
	// check if the whole obstacle is in range
	do{								
		if(cc.distance2(ls[j]) > m_detectRangeSq) break;		// check if the point is in-range
		j++;
	}while(j<ls.size());
	if(j == ls.size()) return true;								// the whole obstacle is in range
	
	
	int i;
	int nexti;
	int arcType;
	btVector3 xpoint1,xpoint2;
	bool outofrange = true;
	
	i=0;
	while(i<ls.size())
	{
		if(i == ls.size()-1) nexti = 0;
		else nexti = i+1;
		
		arcType = arcIntersection(cc, m_detectRange, ls[i], ls[nexti], &xpoint1, &xpoint2);
		
		if(arcType == 0)														// NO intersection
			i++;
		else if(arcType == 1 || arcType == 2){									// ONE intersection
			if(arcType == 2) xpoint1 = xpoint2;
			
			if(nexti == 0) ls.append(xpoint1);									// append new point ls if at the wraparound point
			else ls.insert(nexti,xpoint1);										// insert the intersection point to the ls
			i+=2;
			outofrange = false;
		}
		else{																	// TWO intersections
			if(nexti == 0){														// add the two intersect points between i and nexti
				if(ls[i].distance2(xpoint1) < ls[i].distance2(xpoint2)){		// append them in order
					ls << xpoint1;
					ls << xpoint2;
				}
				else{
					ls << xpoint2;
					ls << xpoint1;
				}
			}
			else{
				if(ls[i].distance2(xpoint1) < ls[i].distance2(xpoint2)){		// insert them in order
					ls.insert(nexti,xpoint1);
					ls.insert(nexti+1,xpoint2);
				}
				else{
					ls.insert(nexti,xpoint2);
					ls.insert(nexti+1,xpoint1);	
				}
			}
			i+=3;
			outofrange = false;
		}
	}
	if(outofrange) return false;												// the entire obstacle is out of range
	
	// remove any points outside of the sensor range
	i=0;
	while(i<ls.size()){
		if(cc.distance2(ls[i]) > m_detectRangeSq+0.001)
			ls.removeAt(i);
		else
			i++;
	}

	if(ls.size() == 2){
		btVector3 cx;
		cx = (ls[1] - ls[0]).cross((ls[0]-btVector3(0,0,1)) - ls[0]);
		cx.normalize();
		ls << (ls[1]+(cx*0.1));
		ls << (ls[0]+(cx*0.1));
	}
	return true;
}