point3d.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *  Software License Agreement (BSD License)
00003 *  Copyright (c) 2013, Intelligent Robotics Lab, DLUT.
00004 *  All rights reserved.
00005 *  Author:Zhao Cilang,Yan Fei,Zhuang Yan
00006 *  Redistribution and use in source and binary forms, with or without
00007 *  modification, are permitted provided that the following conditions
00008 *  are met:
00009 *
00010 *   * Redistributions of source code must retain the above copyright
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Intelligent Robotics Lab nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033 #include "camera_laser_calibration/basic_data/point3d.h"
00034 #include <cmath>
00036 CPoint3d::CPoint3d (void):x (0), y (0), z (0)
00037 {
00038 }
00039 
00040 CPoint3d::CPoint3d (float x, float y, float z)
00041 {
00042   this->x = x;
00043   this->y = y;
00044   this->z = z;
00045 }
00046 
00047 CPoint3d::~CPoint3d (void)
00048 {
00049 }
00050 
00051 CPoint3d::CPoint3d (const CPoint3d & tmpP)
00052 {
00053   this->x = tmpP.x;
00054   this->y = tmpP.y;
00055   this->z = tmpP.z;
00056 }
00057 
00059 CPoint3d & CPoint3d::operator += (const CPoint3d & p)
00060 {
00061   this->x += p.x;
00062   this->y += p.y;
00063   this->z += p.z;
00064   
00065   return *this;
00066 }
00067 
00068 CPoint3d & CPoint3d::operator -= (const CPoint3d & p)
00069 {
00070   this->x -= p.x;
00071   this->y -= p.y;
00072   this->z -= p.z;
00073   
00074   return *this;
00075 }
00076 
00077 CPoint3d & CPoint3d::operator *= (float s)
00078 {
00079   this->x *= s;
00080   this->y *= s;
00081   this->z *= s;
00082   
00083   return *this;
00084 }
00085 
00086 CPoint3d & CPoint3d::operator /= (float s)
00087 {
00088   this->x /= s;
00089   this->y /= s;
00090   this->z /= s;
00091   
00092   return *this;
00093 }
00094 
00095 
00096 CPoint3d operator + (const CPoint3d & p1, const CPoint3d & p2)
00097 {
00098   CPoint3d po;
00099   po.x = p1.x + p2.x;
00100   po.y = p1.y + p2.y;
00101   po.z = p1.z + p2.z;
00102   
00103   return po;
00104 }
00105 
00106 CPoint3d operator - (const CPoint3d & p1, const CPoint3d & p2)
00107 {
00108   CPoint3d po;
00109   po.x = p1.x - p2.x;
00110   po.y = p1.y - p2.y;
00111   po.z = p1.z - p2.z;
00112   
00113   return po;
00114 }
00115 
00116 CPoint3d operator * (const CPoint3d & p, float s)
00117 {
00118   CPoint3d po;
00119   po.x = p.x * s;
00120   po.y = p.y * s;
00121   po.z = p.z * s;
00122   
00123   return po;
00124 }
00125 
00126 float operator * (const CPoint3d & p1, const CPoint3d & p2)
00127 {
00128   return (p1.x * p2.x + p1.y * p2.y + p1.z * p2.z);
00129 }
00130 
00131 CPoint3d operator / (const CPoint3d & p, float num)
00132 {
00133   if (num != 0)
00134   {
00135     CPoint3d po;
00136     po.x = p.x / num;
00137     po.y = p.y / num;
00138     po.z = p.z / num;
00139     
00140     return po;
00141   }
00142   else
00143   {
00144     return CPoint3d (0, 0, 0);
00145   }
00146 }
00147 
00148 CPoint3d operator ^ (const CPoint3d & p1, const CPoint3d & p2)
00149 {
00150   CPoint3d po (p1.y * p2.z - p1.z * p2.y, p1.z * p2.x - p1.x * p2.z, p1.x * p2.y - p1.y * p2.x);
00151   
00152   return po;
00153 }
00154 
00155 bool operator < (const CPoint3d & p1, const CPoint3d & p2)
00156 {
00157   return (p1.z != p2.z) ? (p1.z < p2.z) : (p1.y != p2.y) ? (p1.y < p2.y) : (p1.x < p2.x);
00158 }
00159 
00160 bool operator > (const CPoint3d & p1, const CPoint3d & p2)
00161 {
00162   return (p1.z != p2.z) ? (p1.z > p2.z) : (p1.y != p2.y) ? (p1.y > p2.y) : (p1.x > p2.x);
00163 }
00164 
00165 bool operator == (const CPoint3d & p1, const CPoint3d & p2)
00166 {
00167   if (p1.x == p2.x && p1.y == p2.y && p1.z == p2.z)
00168     return true;
00169   else
00170     return false;
00171 }
00172 
00173 bool operator != (const CPoint3d & p1, const CPoint3d & p2)
00174 {
00175   if (p1.x == p2.x && p1.y == p2.y && p1.z == p2.z)
00176     return false;
00177   else
00178     return true;
00179 }
00180 
00181 
00183 float CPoint3d::cpDist (void) const 
00184 {
00185   return sqrt (x * x + y * y + z * z);
00186 }
00187 
00188 
00189 float CPoint3d::cpSquaredNorm () const 
00190 {
00191   return (this->x * this->x + this->y * this->y + this->z * this->z);
00192 }
00193 
00194 
00195 float CPoint3d::cpDot (const CPoint3d & p) const 
00196 {
00197   return (*this) * p;
00198 }
00199 
00200 
00201 CPoint3d & CPoint3d::cpNormalize ()
00202 {
00203   float n = float (sqrt (this->x * this->x + this->y * this->y + this->z * this->z));
00204   if (n > float (0))
00205   {
00206     this->x /= n;
00207     this->y /= n;
00208     this->z /= n;
00209   }
00210   
00211   return *this;
00212 }


camera_laser_calibration
Author(s): Zhao Cilang,Yan Fei,Zhuang Yan/zhuang@dlut.edu.cn
autogenerated on Sun Jan 5 2014 11:05:02