Cc3dVector4 componentProduct(const Cc3dVector4&v1,const Cc3dVector4&v2){
Cc3dVector4 v(v1.x()*v2.x(),
v1.y()*v2.y(),
v1.z()*v2.z(),
v1.w()*v2.w());
return v;
}
Cc3dVector4 cross(const Cc3dVector4&v1,const Cc3dVector4&v2)
{
assert(v1.w()==0);
assert(v2.w()==0);
Cc3dVector4 rs(v1.y()*v2.z()-v1.z()*v2.y(),
v1.z()*v2.x()-v1.x()*v2.z(),
v1.x()*v2.y()-v1.y()*v2.x(),
0);//cross product result must be a vector, so the fourth component set to zero
return rs;
}
Cc3dVector4 normalize(const Cc3dVector4&v){
//assert(v.w()==0);
float r2=v.x()*v.x()+v.y()*v.y()+v.z()*v.z();
if(r2==0){
return Cc3dVector4(0,0,0,0);
}
float r=sqrtf(r2);
Cc3dVector4 rs(v.x()/r,v.y()/r,v.z()/r,0);
return rs;
}
void Coctree::getpCollisionLeafList_inn(CocNode*pNode,const Cc3dVector4&c,float R, vector<CocNode*>&pCollisionLeafList)
//获得与c为球心R为半径球体的碰撞叶子节点
{
if(pNode==NULL)return;
//检查球体cR是否与pNode碰撞
bool collision=false;
{
const float *c_node=pNode->getCenter().getArray();
const float R_node=pNode->getBoundingSphereRadius();
float d2=square(c_node[0]-c.x())+square(c_node[1]-c.y())+square(c_node[2]-c.z());
if(d2<square(R+R_node)){//相交
collision=true;
}
}//得到collision
if(collision){//如果碰撞
if(pNode->getIsLeaf()){
//将pNode加入到pCollisionLeafList
pCollisionLeafList.push_back(pNode);
}else{
//去考察pNode的孩子
for(int i=0;i<8;i++){
getpCollisionLeafList_inn(pNode->getChildByIndex(i),c,R,pCollisionLeafList);
}
}
}
}
float dot(const Cc3dVector4&v1,const Cc3dVector4&v2){
return v1.x()*v2.x()+v1.y()*v2.y()+v1.z()*v2.z();
}
float getLength(const Cc3dVector4&v){
assert(v.w()==0);
return sqrtf(v.x()*v.x()+v.y()*v.y()+v.z()*v.z());
}
float getLength2(const Cc3dVector4&v){//square of length
assert(v.w()==0);
return v.x()*v.x()+v.y()*v.y()+v.z()*v.z();
}
void Cc3dTransform::setPos(const Cc3dVector4&pos){
setPos(pos.x(),pos.y(),pos.z());
}
Cc3dVector4 Cc3dVector4::operator -(const Cc3dVector4&right)const {
Cc3dVector4 rs(this->x()-right.x(),this->y()-right.y(),this->z()-right.z(),this->w()-right.w());
return rs;
}
