Radar Problem

MatrixF GuiRadar::calcRadarCoords(MatrixF source, MatrixF target)
{
   MatrixF invTransform(source);
   invTransform.inverse();
   MatrixF objTransform(target);
   objTransform.mul(invTransform);
   return objTransform;
}

void GuiRadar::onRender(Point2I offset, const RectI &updateRect)
{
   GameConnection* conn = GameConnection::getServerConnection();
   if (!conn) return;
   Vehicle* control = dynamic_cast<Vehicle*>(conn->getControlObject());
   if (!control) return;
   RectI border(offset.x,offset.y,mBounds.extent.x,mBounds.extent.y);
   setUpdate();
   if (mTextureHandle)
      GFX->drawBitmapStretch(mTextureHandle, border);
   gClientContainer.initRadiusSearch(control->getPosition(),1000,VehicleObjectType);
   U32 targetobject;
   while ((targetobject = gClientContainer.containerSearchNext()) != 0) {
      if (targetobject != control->getId()) {
         Vehicle* target = static_cast<Vehicle*>(Sim::findObject(targetobject));
         MatrixF radar = calcRadarCoords(control->getTransform(),target->getTransform
());
         Point3F rad;
         radar.getColumn(3,&rad);
         Point2F rad2(rad.x,rad.z);
         rad.normalize();
         rad2.normalize();
         rad2.x *= (1 - rad.y);
         rad2.y *= (1 - rad.y);
         rad2 /= 2;
         rad2.x *= -1;
         rad2.x += 1;
         rad2.x /= 2;
         rad2.y += 1;
         rad2.y /= 2;
         rad2.x *= -1;
         rad2.y *= -1;
         rad2.x += 1;
         rad2.y += 1;
         rad2.x *= mBounds.extent.x;
         rad2.y *= mBounds.extent.y;
         rad2.x += offset.x;
         rad2.y += offset.y; 
         Point2I normal(rad2.x+0.5,rad2.y+0.5);
         F32 radius = 2;
         ColorI farbe(0,0,0,0);
         if (control->getTarget() == targetobject) { 
            farbe = ColorI(255,255,255,255);
            radius = 4;
            GFX->drawCircle(normal,radius,farbe,0,4);
            radius -= 2;
            GFX->drawCircle(normal,radius,farbe,0,4);
         }
         else {
            if (target->getTeam() == 0) farbe = ColorI(255,255,0,255);
            else if (target->getTeam() == control->getTeam() && target->getTypeS() ==
 6) farbe = ColorI(0,255,0,255);
            else if (target->getTeam() == control->getTeam()) farbe = ColorI(0,200,0,
255);
            else if (target->getTypeS() == 6) farbe = ColorI(200,0,0,255);
            else farbe = ColorI(255,0,0,255);
            if (target->getTypeS() == 6) radius = 3;
            GFX->drawCircle(normal,radius,farbe,0,4);
         }
      }
   }
   renderChildControls(offset, updateRect);
}