29IMETHOD Vector::Vector(
const Vector & arg)
31 data[0] = arg.data[0];
32 data[1] = arg.data[1];
33 data[2] = arg.data[2];
36IMETHOD Vector::Vector(
double x,
double y,
double z)
41IMETHOD Vector::Vector(
double* xyz)
46IMETHOD Vector::Vector(
float* xyz)
51IMETHOD void Vector::GetValue(
double* xyz)
const
59 data[0] = arg.data[0];
60 data[1] = arg.data[1];
61 data[2] = arg.data[2];
68 tmp.data[0] = lhs.data[0]+rhs.data[0];
69 tmp.data[1] = lhs.data[1]+rhs.data[1];
70 tmp.data[2] = lhs.data[2]+rhs.data[2];
77 tmp.data[0] = lhs.data[0]-rhs.data[0];
78 tmp.data[1] = lhs.data[1]-rhs.data[1];
79 tmp.data[2] = lhs.data[2]-rhs.data[2];
87IMETHOD void Vector::x(
double _x ) {
data[0] = _x; }
88IMETHOD void Vector::y(
double _y ) {
data[1] = _y; }
89IMETHOD void Vector::z(
double _z ) {
data[2] = _z; }
94 tmp.data[0] = lhs.data[0]*rhs;
95 tmp.data[1] = lhs.data[1]*rhs;
96 tmp.data[2] = lhs.data[2]*rhs;
103 tmp.data[0] = lhs*rhs.data[0];
104 tmp.data[1] = lhs*rhs.data[1];
105 tmp.data[2] = lhs*rhs.data[2];
112 tmp.data[0] = lhs.data[0]/rhs;
113 tmp.data[1] = lhs.data[1]/rhs;
114 tmp.data[2] = lhs.data[2]/rhs;
122 tmp.data[0] = lhs.data[1]*rhs.data[2]-lhs.data[2]*rhs.data[1];
123 tmp.data[1] = lhs.data[2]*rhs.data[0]-lhs.data[0]*rhs.data[2];
124 tmp.data[2] = lhs.data[0]*rhs.data[1]-lhs.data[1]*rhs.data[0];
128Vector& Vector::operator +=(
const Vector & arg)
131 data[0]+=arg.data[0];
132 data[1]+=arg.data[1];
133 data[2]+=arg.data[2];
137Vector& Vector::operator -=(
const Vector & arg)
140 data[0]-=arg.data[0];
141 data[1]-=arg.data[1];
142 data[2]-=arg.data[2];
146Vector& Vector::operator *=(
double arg)
159double Vector::operator()(
int index)
const {
164double& Vector::operator () (
int index)
173 return (l<
eps) ? Vector(0.0,0.0,0.0) : a/l;
176Wrench Frame::operator * (
const Wrench& arg)
const
180 tmp.force =
M*arg.force;
181 tmp.torque =
M*arg.torque + p*tmp.force;
185Wrench Frame::Inverse(
const Wrench& arg)
const
188 tmp.force =
M.Inverse(arg.force);
189 tmp.torque =
M.Inverse(arg.torque-p*arg.force);
195Wrench Rotation::Inverse(
const Wrench& arg)
const
197 return Wrench(Inverse(arg.force),Inverse(arg.torque));
200Twist Rotation::Inverse(
const Twist& arg)
const
202 return Twist(Inverse(arg.vel),Inverse(arg.rot));
207 return Wrench(Vector::Zero(),Vector::Zero());
211void Wrench::ReverseSign()
213 torque.ReverseSign();
217Wrench Wrench::RefPoint(
const Vector& v_base_AB)
const
223 return Wrench(this->force,
224 this->torque+this->force*v_base_AB
229Wrench& Wrench::operator-=(
const Wrench& arg)
236Wrench& Wrench::operator+=(
const Wrench& arg)
243double& Wrench::operator()(
int i)
252double Wrench::operator()(
int i)
const
264 return Wrench(lhs.force*rhs,lhs.torque*rhs);
269 return Wrench(lhs*rhs.force,lhs*rhs.torque);
274 return Wrench(lhs.force/rhs,lhs.torque/rhs);
280 return Wrench(lhs.force+rhs.force,lhs.torque+rhs.torque);
285 return Wrench(lhs.force-rhs.force,lhs.torque-rhs.torque);
291 return Wrench(-arg.force,-arg.torque);
294Twist Frame::operator * (
const Twist& arg)
const
299 tmp.vel =
M*arg.vel+p*tmp.rot;
302Twist Frame::Inverse(
const Twist& arg)
const
305 tmp.rot =
M.Inverse(arg.rot);
306 tmp.vel =
M.Inverse(arg.vel-p*arg.rot);
312 return Twist(Vector::Zero(),Vector::Zero());
316void Twist::ReverseSign()
322Twist Twist::RefPoint(
const Vector& v_base_AB)
const
329 return Twist(this->vel+this->
rot*v_base_AB,this->
rot);
332Twist& Twist::operator-=(
const Twist& arg)
339Twist& Twist::operator+=(
const Twist& arg)
346double& Twist::operator()(
int i)
355double Twist::operator()(
int i)
const
367 return Twist(lhs.vel*rhs,lhs.rot*rhs);
372 return Twist(lhs*rhs.vel,lhs*rhs.rot);
377 return Twist(lhs.vel/rhs,lhs.rot/rhs);
383 return Twist(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
388 return Twist(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
394 return Twist(-arg.vel,-arg.rot);
397Frame::Frame(
const Rotation &
R)
403Frame::Frame(
const Vector &
V)
405 M = Rotation::Identity();
409Frame::Frame(
const Rotation &
R,
const Vector &
V)
418 return Frame(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
421Vector Frame::operator *(
const Vector & arg)
const
426Vector Frame::Inverse(
const Vector& arg)
const
428 return M.Inverse(arg-p);
431Frame Frame::Inverse()
const
433 return Frame(
M.Inverse(),-
M.Inverse(p));
437Frame& Frame::operator =(
const Frame & arg)
444Frame::Frame(
const Frame & arg) :
449void Vector::ReverseSign()
461 tmp.data[0]=-arg.data[0];
462 tmp.data[1]=-arg.data[1];
463 tmp.data[2]=-arg.data[2];
467void Vector::Set2DXY(
const Vector2& v)
475void Vector::Set2DYZ(
const Vector2& v)
484void Vector::Set2DZX(
const Vector2& v)
497double& Rotation::operator()(
int i,
int j) {
502double Rotation::operator()(
int i,
int j)
const {
507Rotation::Rotation(
double Xx,
double Yx,
double Zx,
508 double Xy,
double Yy,
double Zy,
509 double Xz,
double Yz,
double Zz)
517Rotation::Rotation(
const Vector&
x,
const Vector&
y,
const Vector&
z)
524Rotation& Rotation::operator=(
const Rotation& arg) {
526 while (count--)
data[count] = arg.data[count];
530Vector Rotation::operator*(
const Vector& v)
const {
533 data[0]*
v.data[0] + data[1]*
v.data[1] + data[2]*
v.data[2],
534 data[3]*
v.data[0] + data[4]*
v.data[1] + data[5]*
v.data[2],
535 data[6]*
v.data[0] + data[7]*
v.data[1] + data[8]*
v.data[2]
539Twist Rotation::operator * (
const Twist& arg)
const
545 return Twist((*
this)*arg.vel,(*
this)*arg.rot);
548Wrench Rotation::operator * (
const Wrench& arg)
const
553 return Wrench((*
this)*arg.force,(*
this)*arg.torque);
556Rotation Rotation::Identity() {
557 return Rotation(1,0,0,0,1,0,0,0,1);
560void Rotation::DoRotX(
double angle)
565 x1 = cs* (*this)(0,1) + sn* (*
this)(0,2);
566 x2 = cs* (*this)(1,1) + sn* (*
this)(1,2);
567 x3 = cs* (*this)(2,1) + sn* (*
this)(2,2);
568 (*this)(0,2) = -sn* (*
this)(0,1) + cs* (*
this)(0,2);
569 (*this)(1,2) = -sn* (*
this)(1,1) + cs* (*
this)(1,2);
570 (*this)(2,2) = -sn* (*
this)(2,1) + cs* (*
this)(2,2);
576void Rotation::DoRotY(
double angle)
581 x1 = cs* (*this)(0,0) - sn* (*
this)(0,2);
582 x2 = cs* (*this)(1,0) - sn* (*
this)(1,2);
583 x3 = cs* (*this)(2,0) - sn* (*
this)(2,2);
584 (*this)(0,2) = sn* (*
this)(0,0) + cs* (*
this)(0,2);
585 (*this)(1,2) = sn* (*
this)(1,0) + cs* (*
this)(1,2);
586 (*this)(2,2) = sn* (*
this)(2,0) + cs* (*
this)(2,2);
592void Rotation::DoRotZ(
double angle)
597 x1 = cs* (*this)(0,0) + sn* (*
this)(0,1);
598 x2 = cs* (*this)(1,0) + sn* (*
this)(1,1);
599 x3 = cs* (*this)(2,0) + sn* (*
this)(2,1);
600 (*this)(0,1) = -sn* (*
this)(0,0) + cs* (*
this)(0,1);
601 (*this)(1,1) = -sn* (*
this)(1,0) + cs* (*
this)(1,1);
602 (*this)(2,1) = -sn* (*
this)(2,0) + cs* (*
this)(2,1);
609Rotation Rotation::RotX(
double angle) {
612 return Rotation(1,0,0,0,cs,-sn,0,sn,cs);
614Rotation Rotation::RotY(
double angle) {
617 return Rotation(cs,0,sn,0,1,0,-sn,0,cs);
619Rotation Rotation::RotZ(
double angle) {
622 return Rotation(cs,-sn,0,sn,cs,0,0,0,1);
628void Frame::Integrate(
const Twist& t_this,
double samplefrequency)
630 double n = t_this.rot.Norm()/samplefrequency;
632 p +=
M*(t_this.vel/samplefrequency);
635 Frame ( Rotation::Rot( t_this.rot, n ),
636 t_this.vel/samplefrequency
641Rotation Rotation::Inverse()
const
648Vector Rotation::Inverse(
const Vector& v)
const {
650 data[0]*
v.data[0] + data[3]*
v.data[1] + data[6]*
v.data[2],
651 data[1]*
v.data[0] + data[4]*
v.data[1] + data[7]*
v.data[2],
652 data[2]*
v.data[0] + data[5]*
v.data[1] + data[8]*
v.data[2]
656void Rotation::setValue(
float* oglmat)
658 data[0] = *oglmat++;
data[3] = *oglmat++;
data[6] = *oglmat++; oglmat++;
659 data[1] = *oglmat++;
data[4] = *oglmat++;
data[7] = *oglmat++; oglmat++;
660 data[2] = *oglmat++;
data[5] = *oglmat++;
data[8] = *oglmat;
664void Rotation::getValue(
float* oglmat)
const
666 *oglmat++ = (
float)data[0]; *oglmat++ = (
float)data[3]; *oglmat++ = (
float)data[6]; *oglmat++ = 0.f;
667 *oglmat++ = (
float)data[1]; *oglmat++ = (
float)data[4]; *oglmat++ = (
float)data[7]; *oglmat++ = 0.f;
668 *oglmat++ = (
float)data[2]; *oglmat++ = (
float)data[5]; *oglmat++ = (
float)data[8]; *oglmat++ = 0.f;
669 *oglmat++ = 0.f; *oglmat++ = 0.f; *oglmat++ = 0.f; *oglmat = 1.f;
672void Rotation::SetInverse()
686double Frame::operator()(
int i,
int j) {
702double Frame::operator()(
int i,
int j)
const {
719Frame Frame::Identity() {
720 return Frame(Rotation::Identity(),Vector::Zero());
724void Frame::setValue(
float* oglmat)
727 p.data[0] = oglmat[12];
728 p.data[1] = oglmat[13];
729 p.data[2] = oglmat[14];
732void Frame::getValue(
float* oglmat)
const
735 oglmat[12] = (
float)p.data[0];
736 oglmat[13] = (
float)p.data[1];
737 oglmat[14] = (
float)p.data[2];
740void Vector::Set2DPlane(
const Frame& F_someframe_XY,
const Vector2& v_XY)
746tmp_XY = F_someframe_XY*(tmp_XY);
758IMETHOD Vector2::Vector2(
const Vector2 & arg)
760 data[0] = arg.data[0];
761 data[1] = arg.data[1];
764IMETHOD Vector2::Vector2(
double x,
double y)
769IMETHOD Vector2::Vector2(
double* xy)
774IMETHOD Vector2::Vector2(
float* xy)
781 data[0] = arg.data[0];
782 data[1] = arg.data[1];
786IMETHOD void Vector2::GetValue(
double* xy)
const
793 return Vector2(lhs.data[0]+rhs.data[0],lhs.data[1]+rhs.data[1]);
798 return Vector2(lhs.data[0]-rhs.data[0],lhs.data[1]-rhs.data[1]);
803 return Vector2(lhs.data[0]*rhs,lhs.data[1]*rhs);
808 return Vector2(lhs*rhs.data[0],lhs*rhs.data[1]);
813 return Vector2(lhs.data[0]/rhs,lhs.data[1]/rhs);
818 data[0]+=arg.data[0];
819 data[1]+=arg.data[1];
825 data[0]-=arg.data[0];
826 data[1]-=arg.data[1];
844IMETHOD void Vector2::ReverseSign()
853 return Vector2(-arg.data[0],-arg.data[1]);
857IMETHOD void Vector2::Set3DXY(
const Vector& v)
863IMETHOD void Vector2::Set3DYZ(
const Vector& v)
869IMETHOD void Vector2::Set3DZX(
const Vector& v)
876IMETHOD void Vector2::Set3DPlane(
const Frame& F_someframe_XY,
const Vector& v_someframe)
880 Vector tmp = F_someframe_XY.Inverse(v_someframe);
887IMETHOD Rotation2& Rotation2::operator=(
const Rotation2& arg) {
893 return Vector2(
v.data[0]*c-
v.data[1]*s,
v.data[0]*s+
v.data[1]*c);
896IMETHOD double Rotation2::operator()(
int i,
int j)
const {
907 return Rotation2(lhs.c*rhs.c-lhs.s*rhs.s,lhs.s*rhs.c+lhs.c*rhs.s);
910IMETHOD void Rotation2::SetInverse() {
914IMETHOD Rotation2 Rotation2::Inverse()
const {
915 return Rotation2(c,-s);
919 return Vector2(
v.data[0]*c+
v.data[1]*s,-
v.data[0]*s+
v.data[1]*c);
922IMETHOD Rotation2 Rotation2::Identity() {
923 return Rotation2(1,0);
926IMETHOD void Rotation2::SetIdentity()
940IMETHOD double Rotation2::GetRot()
const {
948IMETHOD Frame2::Frame2(
const Rotation2 &
R)
954IMETHOD Frame2::Frame2(
const Vector2 &
V)
956 M = Rotation2::Identity();
960IMETHOD Frame2::Frame2(
const Rotation2 &
R,
const Vector2 &
V)
968 return Frame2(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
978 return M.Inverse(arg-p);
981IMETHOD void Frame2::SetIdentity()
987IMETHOD void Frame2::SetInverse()
995IMETHOD Frame2 Frame2::Inverse()
const
1002IMETHOD Frame2& Frame2::operator =(
const Frame2 & arg)
1009IMETHOD Frame2::Frame2(
const Frame2 & arg) :
1014IMETHOD double Frame2::operator()(
int i,
int j) {
1030IMETHOD double Frame2::operator()(
int i,
int j)
const {
1049 return rhs(0)*lhs(0)+rhs(1)*lhs(1)+rhs(2)*lhs(2);
1053 return dot(lhs.vel,rhs.force)+
dot(lhs.rot,rhs.torque);
1057 return dot(lhs.vel,rhs.force)+
dot(lhs.rot,rhs.torque);
1069 return (
Equal(a.data[0],
b.data[0],
eps)&&
1091 return (
Equal(a.data[0],
b.data[0],
eps)&&
1117 v = Vector2::Zero();
1148 Vector rotvec = axis_a_b;
1149 double angle = rotvec.Normalize(1E-10);
1154 ct + vt*rotvec(0)*rotvec(0),
1155 -rotvec(2)*st + vt*rotvec(0)*rotvec(1),
1156 rotvec(1)*st + vt*rotvec(0)*rotvec(2),
1157 rotvec(2)*st + vt*rotvec(1)*rotvec(0),
1158 ct + vt*rotvec(1)*rotvec(1),
1159 -rotvec(0)*st + vt*rotvec(1)*rotvec(2),
1160 -rotvec(1)*st + vt*rotvec(2)*rotvec(0),
1161 rotvec(0)*st + vt*rotvec(2)*rotvec(1),
1162 ct + vt*rotvec(2)*rotvec(2)
1202IMETHOD Vector
diff(
const Rotation& R_a_b1,
const Rotation& R_a_b2,
double dt) {
1203 Rotation R_b1_b2(R_a_b1.Inverse()*R_a_b2);
1204 return R_a_b1 * R_b1_b2.GetRot() / dt;
1206IMETHOD Twist
diff(
const Frame& F_a_b1,
const Frame& F_a_b2,
double dt) {
1208 diff(F_a_b1.p,F_a_b2.p,dt),
1209 diff(F_a_b1.M,F_a_b2.M,dt)
1213 return Twist(
diff(a.vel,
b.vel,dt),
diff(a.rot,
b.rot,dt));
1218 diff(a.force,
b.force,dt),
1219 diff(a.torque,
b.torque,dt)
1229 return a*
Rot(a.Inverse(da)*dt);
1241 return Wrench(
addDelta(a.force,da.force,dt),
addDelta(a.torque,da.torque,dt));
1303 R = Rotation::EulerZYX(alfa,
beta,gamma);
1332 R = Rotation::EulerZYX(alfa,
beta,gamma);
1347 return (a.p ==
b.p &&
1360 return (a.data[0]==
b.data[0]&&
1361 a.data[1]==
b.data[1]&&
1362 a.data[2]==
b.data[2] );
1374 return (a.rot==
b.rot &&
1387 return (a.force==
b.force &&
1388 a.torque==
b.torque );
SIMD_FORCE_INLINE const btScalar & z() const
Return the z value.
SIMD_FORCE_INLINE btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
bool Equal(const VectorAcc &r1, const VectorAcc &r2, double eps)
Vector operator/(const Vector &lhs, double rhs)
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt)
IMETHOD Vector operator-(const Vector &lhs, const Vector &rhs)
IMETHOD void SetToZero(Vector &v)
IMETHOD Vector addDelta(const Vector &a, const Vector &da, double dt)
IMETHOD Vector operator+(const Vector &lhs, const Vector &rhs)
IMETHOD void posrandom(Vector &a)
Vector operator*(const Vector &lhs, double rhs)
IMETHOD Vector Normalize(const Vector &a, double eps)
IMETHOD Rotation Rot(const Vector &axis_a_b)
float dot(VecOp< float, D >, VecOp< float, D >) RET
__forceinline LightTreeMeasure operator+(const LightTreeMeasure &a, const LightTreeMeasure &b)
ccl_device_inline Extrema< T > operator*(const Extrema< T > a, const T b)
ccl_device_inline float beta(const float x, const float y)
ccl_device_inline dual< T > operator/(const dual< T > a, T b)
ccl_device_inline dual< T1 > operator-(const dual< T1 > a, T2 b)
ccl_device_inline bool operator!=(const float2 a, const float2 b)
ccl_device_inline bool operator==(const float2 a, const float2 b)
ATTR_WARN_UNUSED_RESULT const BMVert * v
BMesh const char void * data
KERNEL_FEATURE_LIGHT_PASSES KERNEL_FEATURE_PATH_GUIDING float
CCL_NAMESPACE_BEGIN struct Window V